src/FDM/LaRCsim/c172_main.c

   1 // LaRCsim.cxx -- interface to the LaRCsim flight model
   2 //
   3 // Written by Curtis Olson, started October 1998.
   4 //
   5 // Copyright (C) 1998  Curtis L. Olson  - curt@me.umn.edu
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22
  23
  24
  25
  26
  27 #include <FDM/LaRCsim/ls_cockpit.h>
  28 #include <FDM/LaRCsim/ls_generic.h>
  29 #include <FDM/LaRCsim/ls_interface.h>
  30 #include <FDM/LaRCsim/ls_constants.h>
  31 #include <FDM/LaRCsim/atmos_62.h>
  32 /* #include <FDM/LaRCsim/ls_trim_fs.h> */
  33 #include <FDM/LaRCsim/c172_aero.h>
  34 #include <FDM/LaRCsim/ic.h>
  35
  36 #include <math.h>
  37 #include <stdlib.h>
  38 #include <stdio.h>
  39 #include <string.h>
  40
  41 void do_trims(int kmax,FILE *out,InitialConditions IC)
  42 {
  43         int bad_trim=0,i,j;
  44         double speed,elevator,cmcl,maxspeed;
  45         out=fopen("trims.out","w");
  46         speed=55;
  47
  48         for(j=0;j<=0;j+=10)
  49         {
  50                 IC.flap_handle=j;
  51                 for(i=4;i<=4;i++)
  52                 {
  53                         switch(i)
  54                         {
  55                                 case 1: IC.weight=1500;IC.cg=0.155;break;
  56                                 case 2: IC.weight=1500;IC.cg=0.364;break;
  57                                 case 3: IC.weight=1950;IC.cg=0.155;break;
  58                                 case 4: IC.weight=2400;IC.cg=0.257;break;
  59                                 case 5: IC.weight=2550;IC.cg=0.364;break;
  60                         }
  61
  62                         speed=40;
  63                         if(j > 0) { maxspeed = 90; }
  64                         else { maxspeed = 170; }
  65                         while(speed <= maxspeed)
  66                         {
  67                            IC.vc=speed;
  68                            Long_control=0;Theta=0;Throttle_pct=0.0;
  69
  70                            bad_trim=trim_long(kmax,IC);
  71                            if(Long_control <= 0)
  72                                   elevator=Long_control*28;
  73                            else
  74                                  elevator=Long_control*23;
  75                            if(fabs(CL) > 1E-3)
  76                            {
  77                                         cmcl=cm / CL;
  78                            }
  79                            if(!bad_trim)
  80                            {
  81                                         fprintf(out,"%g,%g,%g,%g,%g",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Flap_Position);
  82                                         fprintf(out,",%g,%g,%g,%g,%g\n",CL,cm,cmcl,Weight,Cg);
  83         /*                              printf("%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n",V_calibrated_kts,Alpha*RAD_TO_DEG,elevator,CL,cm,Cmo,Cma,Cmde,Mass*32.174,Dx_cg);
  84          */                }
  85                    else
  86                            {
  87                                  printf("kmax exceeded at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_true_kts,Weight,Cg,Flap_Position);
  88                                  printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
  89                  printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
  90                            }
  91                            speed+=10;
  92                         }
  93         }
  94         }
  95         fclose(out);
  96 }
  97
  98 find_max_alt(int kmax,InitialConditions IC)
  99 {
 100         int bad_trim=0,i=0;
 101         float min=0,max=30000;
 102         IC.use_gamma_tmg=1;
 103         IC.gamma=0;
 104         IC.vc=73;
 105         IC.altitude==1000;
 106         while(!bad_trim)
 107         {
 108                 bad_trim=trim_long(200,IC);
 109                 IC.altitude+=1000;
 110         }
 111         while((fabs(max-min) > 100) && (i < 50))
 112         {
 113
 114                 IC.altitude=(max-min)/2 + min;
 115                 printf("\nIC.altitude: %g, max: %g, min: %g, bad_trim: %d\n",IC.altitude,max,min,bad_trim);
 116                 printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
 117
 118                 bad_trim=trim_long(200,IC);
 119
 120                 if(bad_trim == 1 )
 121                         max=IC.altitude;
 122                 else
 123                         min=IC.altitude;
 124                 i++;
 125         }
 126 }
 127
 128
 129 void find_trim_stall(int kmax,FILE *out,InitialConditions IC)
 130 {
 131         int k=0,i,j;
 132         int failf;
 133         char axis[10];
 134         double speed,elevator,cmcl,speed_inc,lastgood;
 135         out=fopen("trim_stall.summary","w");
 136         speed=90;
 137         speed_inc=10;
 138         //failf=malloc(sizeof(int));
 139
 140         for(j=0;j<=30;j+=10)
 141         {
 142                 IC.flap_handle=j;
 143                 for(i=1;i<=6;i++)
 144                 {
 145                         switch(i)
 146                         {
 147                                 case 1: IC.weight=1500;IC.cg=0.155;break;
 148                                 case 2: IC.weight=1500;IC.cg=0.364;break;
 149                                 case 3: IC.weight=2400;IC.cg=0.155;break;
 150                                 case 4: IC.weight=2400;IC.cg=0.364;break;
 151                                 case 5: IC.weight=2550;IC.cg=0.257;break;
 152                                 case 6: IC.weight=2550;IC.cg=0.364;break;
 153                         }
 154
 155                         speed=90;
 156                         speed_inc=10;
 157                         while(speed_inc >= 0.5)
 158                         {
 159                            IC.vc=speed;
 160                            Long_control=0;Theta=0;Throttle_pct=0.0;
 161                            failf=trim_longfr(kmax,IC);
 162                            if(Long_control <= 0)
 163                                   elevator=Long_control*28;
 164                            else
 165                                  elevator=Long_control*23;
 166                            if(fabs(CL) > 1E-3)
 167                            {
 168                                         cmcl=cm / CL;
 169                            }
 170                            if(failf == 0)
 171                            {
 172                                         lastgood=speed;
 173                                         axis[0]='\0';
 174                                         //fprintf(out,"%g,%g,%g,%g,%g,%d",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Flap_Position,k);
 175                                         //fprintf(out,",%g,%g,%g,%g,%g\n",CL,cm,cmcl,Weight,Cg);
 176         /*                              printf("%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n",V_calibrated_kts,Alpha*RAD_TO_DEG,elevator,CL,cm,Cmo,Cma,Cmde,Mass*32.174,Dx_cg);
 177          */                }
 178                    else
 179                            {
 180                                  printf("trim failed at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_calibrated_kts,Weight,Cg,Flap_Position);
 181                                  printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
 182                  printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
 183                                  printf("Speed increment: %g\n",speed_inc);
 184                                  speed+=speed_inc;
 185                                  speed_inc/=2;
 186                            }
 187                            speed-=speed_inc;
 188
 189
 190                         }
 191                         printf("failf %d\n",failf);
 192                         if(failf == 1)
 193                            strcpy(axis,"lift");
 194                         else if(failf == 2)
 195                            strcpy(axis,"thrust");
 196                         else if(failf == 3)
 197                            strcpy(axis,"pitch");
 198                         fprintf(out,"Last good speed: %g, Flaps: %g, Weight: %g, CG: %g, failed axis: %s\n",lastgood,Flap_handle,Weight,Cg,axis);
 199
 200
 201         }
 202         }
 203         fclose(out);
 204         //free(failf);
 205 }
 206
 207
 208 // Initialize the LaRCsim flight model, dt is the time increment for
 209 // each subsequent iteration through the EOM
 210 int fgLaRCsimInit(double dt) {
 211     ls_toplevel_init(dt);
 212
 213     return(1);
 214 }
 215
 216 int wave_stats(float *var,float *var_rate,int N,FILE *out)
 217 {
 218         int Nc,i,Nmaxima;
 219         float varmax,slope,intercept,time,ld,zeta,omegad,omegan;
 220         float varmaxima[100],vm_times[100];
 221         /*adjust N so that any constant slope region at the end is cut off */
 222         i=N;
 223         while((fabs(var_rate[N]-var_rate[i]) < 0.1) && (i >= 0))
 224         {
 225        i--;
 226         }
 227         Nc=N-i;
 228         slope=(var[N]-var[Nc])/(N*0.01 - Nc*0.01);
 229         intercept=var[N]-slope*N*0.01;
 230         printf("\tRotating constant decay out of data using:\n");
 231         printf("\tslope: %g, intercept: %g\n",slope,intercept);
 232         printf("\tUsing first %d points for dynamic response analysis\n",Nc);
 233         varmax=0;
 234         Nmaxima=0;i=0;
 235         while((i <= Nc) && (i <= 801))
 236         {
 237
 238                 fprintf(out,"%g\t%g",i*0.01,var[i]);
 239                 var[i]-=slope*i*0.01+intercept;
 240                 /* printf("%g\n",var[i]); */
 241         fprintf(out,"\t%g\n",var[i]);
 242                 if(var[i] > varmax)
 243             {
 244                    varmax=var[i];
 245                    time=i*0.01;
 246
 247                 }
 248             if((var[i-1]*var[i] < 0) && (var[i] > 0))
 249                 {
 250                    varmaxima[Nmaxima]=varmax;
 251                    vm_times[Nmaxima]=time;
 252                    printf("\t%6.2f: %8.4f\n",vm_times[Nmaxima],varmaxima[Nmaxima]);
 253                    varmax=0;Nmaxima++;
 254
 255                 }
 256
 257                 i++;
 258     }
 259         varmaxima[Nmaxima]=varmax;
 260     vm_times[Nmaxima]=time;
 261     Nmaxima++;
 262         if(Nmaxima > 2)
 263         {
 264           ld=log(varmaxima[1]/varmaxima[2]);   //logarithmic decrement
 265           zeta=ld/sqrt(4*PI*PI +ld*ld);        //damping ratio
 266           omegad=1/(vm_times[2]-vm_times[1]);  //damped natural frequency Hz
 267           if(zeta < 1)
 268           {
 269                 omegan=omegad/sqrt(1-zeta*zeta);   //natural frequency Hz
 270           }
 271           printf("\tDamping Ratio: %g\n",zeta);
 272           printf("\tDamped Freqency: %g Hz\n\tNatural Freqency: %g Hz\n",omegad,omegan);
 273         }
 274         else
 275           printf("\tNot enough points to take log decrement\n");
 276 /*      printf("w: %g, u: %g, q: %g\n",W_body,U_body,Q_body);
 277  */
 278         return 1;
 279 }
 280
 281 // Run an iteration of the EOM (equations of motion)
 282 int main(int argc, char *argv[]) {
 283
 284
 285         double save_alt = 0.0;
 286     int multiloop=1,k=0,i,j,touchdown,N;
 287         double time=0,elev_trim,elev_trim_save,elevator,speed,cmcl;
 288         FILE *out;
 289         double hgain,hdiffgain,herr,herrprev,herr_diff,htarget;
 290         double lastVt,vtdots,vtdott;
 291         InitialConditions IC;
 292     SCALAR *control[7];
 293         SCALAR *state[7];
 294         float old_state,effectiveness,tol,delta_state,lctrim;
 295         float newcm,lastcm,cmalpha,td_vspeed,td_time,stop_time;
 296         float h[801],hdot[801],altmin,lastAlt,theta[800],theta_dot[800];
 297
 298     if(argc < 6)
 299         {
 300             printf("Need args: $c172 speed alt alpha elev throttle\n");
 301                 exit(1);
 302         }
 303         initIC(&IC);
 304
 305         IC.latitude=47.5299892; //BFI
 306         IC.longitude=122.3019561;
 307         Runway_altitude =   18.0;
 308
 309         IC.altitude=strtod(argv[2],NULL);
 310         printf("h: %g, argv[2]: %s\n",IC.altitude,argv[2]);
 311         IC.vc=strtod(argv[1],NULL);
 312         IC.alpha=0;
 313         IC.beta=0;
 314         IC.theta=strtod(argv[3],NULL);
 315         IC.use_gamma_tmg=0;
 316         IC.phi=0;
 317         IC.psi=0;
 318         IC.weight=2400;
 319         IC.cg=0.25;
 320         IC.flap_handle=10;
 321         IC.long_control=0;
 322         IC.rudder_pedal=0;
 323
 324
 325         ls_ForceAltitude(IC.altitude);
 326     fgLaRCsimInit(0.01);
 327         setIC(IC);
 328         printf("Dx_cg: %g\n",Dx_cg);
 329         V_down=strtod(argv[4],NULL);;
 330         ls_loop(0,-1);
 331         i=0;time=0;
 332         IC.long_control=0;
 333         altmin=Altitude;
 334     printf("\tAltitude: %g, Theta: %g, V_down: %g\n\n",Altitude,Theta*RAD_TO_DEG,V_down);
 335
 336         while(time < 5.0)
 337         {
 338                 printf("Time: %g, Flap_handle: %g, Flap_position: %g, Transit: %d\n",time,Flap_handle,Flap_Position,Flaps_In_Transit);
 339                 if(time > 2.5)
 340                   Flap_handle=20;
 341                 else if (time > 0.5)
 342                   Flap_handle=20;
 343                 ls_update(1);
 344             time+=0.01;
 345     }
 346
 347
 348
 349         /*out=fopen("drop.out","w");
 350         N=800;touchdown=0;
 351
 352         while(i <= N)
 353         {
 354           ls_update(1);
 355           printf("\tAltitude: %g, Theta: %g, V_down: %g\n\n",D_cg_above_rwy,Theta*RAD_TO_DEG,V_down);
 356           fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground);
 357           h[i]=D_cg_above_rwy;hdot[i]=V_down;
 358           theta[i]=Theta; theta_dot[i]=Theta_dot;
 359           if(D_cg_above_rwy < altmin)
 360                 altmin=D_cg_above_rwy;
 361           if((F_Z_gear < -10) && (! touchdown))
 362           {
 363                         touchdown=1;
 364                         td_vspeed=V_down;
 365                         td_time=time;
 366           }
 367           time+=0.01;
 368           i++;
 369         }
 370         while(V_rel_ground > 1)
 371         {
 372                 if(Brake_pct < 1)
 373                 {
 374                    Brake_pct+=0.02;
 375                 }
 376                 ls_update(1);
 377                 time=i*0.01;
 378             fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground);
 379                 i++;
 380     }
 381         stop_time=time;
 382     while((time-stop_time) < 5.0)
 383         {
 384                 ls_update(1);
 385                 time=i*0.01;
 386             fprintf(out,"%g\t%g\t%g\t%g\t%g\t%g\n",time,D_cg_above_rwy,Theta*RAD_TO_DEG,V_down,F_Z_gear/1000.0,V_rel_ground);
 387                 i++;
 388         }
 389         fclose(out);
 390
 391         printf("Min Altitude: %g, Final Alitutde: %g, Delta: %g\n",altmin, h[N],  D_cg_above_rwy-altmin);
 392         printf("Vertical Speed at touchdown: %g, Time at touchdown: %g\n",td_vspeed,td_time);
 393     printf("\nAltitude response:\n");
 394         out=fopen("alt.out","w");
 395         wave_stats(h,hdot,N,out);
 396         fclose(out);
 397         out=fopen("theta.out","w");
 398         printf("\nPitch Attitude response:\n");
 399         wave_stats(theta,theta_dot,N,out);
 400     fclose(out);*/
 401
 402
 403
 404         /*printf("Flap_handle: %g, Flap_Position: %g\n",Flap_handle,Flap_Position);
 405         printf("k: %d, %g knots, %g lbs, %g %%MAC\n",k,V_calibrated_kts,Weight,Cg);
 406         printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
 407     printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha,Throttle_pct,Long_control);
 408
 409         printf("Cme: %g, elevator: %g, Cmde: %g\n",elevator*Cmde,elevator,Cmde);
 410          */
 411
 412
 413
 414
 415
 416
 417
 418         /* ls_loop(0.0,-1);
 419
 420         control[1]=&IC.long_control;
 421         control[2]=&IC.throttle;
 422         control[3]=&IC.alpha;
 423         control[4]=&IC.beta;
 424         control[5]=&IC.phi;
 425         control[6]=&IC.lat_control;
 426
 427         state[1]=&Q_dot_body;state[2]=&U_dot_body;state[3]=&W_dot_body;
 428         state[4]=&R_dot_body;state[5]=&V_dot_body;state[6]=&P_dot_body;
 429
 430
 431         for(i=1;i<=6;i++)
 432         {
 433                 old_state=*state[i];
 434             tol=1E-4;
 435                 for(j=1;j<=6;j++)
 436                 {
 437                         *control[j]+=0.1;
 438                         setIC(IC);
 439                         ls_loop(0.0,-1);
 440                         delta_state=*state[i]-old_state;
 441                         effectiveness=(delta_state)/ 0.1;
 442                         if(delta_state < tol)
 443                                 effectiveness = 0;
 444                         printf("%8.4f,",delta_state);
 445                         *control[j]-=0.1;
 446
 447                 }
 448                 printf("\n");
 449                 setIC(IC);
 450                 ls_loop(0.0,-1);
 451         }                */
 452
 453             return 1;
 454 }
 455
 456 /*
 457 void do_stick_pull(int kmax, SCALAR tmax,FILE *out,InitialConditions IC)
 458 {
 459
 460         SCALAR htarget,hgain,hdiffgain,herr,herr_diff,herrprev;
 461         SCALAR theta_trim,elev_trim,time;
 462         int k;
 463         k=trim_long(kmax,IC);
 464         printf("Trim:\n\tAlpha: %10.6f, elev: %10.6f, Throttle: %10.6f\n\twdot: %10.6f, qdot: %10.6f, udot: %10.6f\n",Alpha*RAD_TO_DEG,Long_control,Throttle_pct,W_dot_body,U_dot_body,Q_dot_body);
 465
 466
 467         htarget=0;
 468
 469         hgain=1;
 470         hdiffgain=1;
 471         elev_trim=Long_control;
 472         out=fopen("stick_pull.out","w");
 473         herr=Q_body-htarget;
 474
 475         //fly steady-level for 2 seconds, well, zero pitch rate anyway
 476         while(time < 2.0)
 477         {
 478                  herrprev=herr;
 479                  ls_update(1);
 480                  herr=Q_body-htarget;
 481                  herr_diff=herr-herrprev;
 482                  Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
 483                  time+=0.01;
 484                  //printf("Time: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, Phi: %7.4f, Psi: %7.4f\n",time,Altitude,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,Phi,Psi);
 485          //printf("Mcg: %7.4f, Mrp: %7.4f, Maero: %7.4f, Meng: %7.4f, Mgear: %7.4f, Dx_cg: %7.4f, Dz_cg: %7.4f\n\n",M_m_cg,M_m_rp,M_m_aero,M_m_engine,M_m_gear,Dx_cg,Dz_cg);
 486                  fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_true_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
 487                  fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 488         }
 489
 490         //begin untrimmed climb at theta_trim + 2 degrees
 491         hgain=4;
 492         hdiffgain=2;
 493         theta_trim=Theta;
 494         htarget=theta_trim;
 495         herr=Theta-htarget;
 496         while(time < tmax)
 497         {
 498                 //ramp in the target theta
 499                 if(htarget < (theta_trim + 2*DEG_TO_RAD))
 500                 {
 501                         htarget+= 0.01*DEG_TO_RAD;
 502                 }
 503                 herrprev=herr;
 504                  ls_update(1);
 505                  herr=Theta-htarget;
 506                  herr_diff=herr-herrprev;
 507                  Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
 508                  time+=0.01;
 509                  //printf("Time: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, Phi: %7.4f, Psi: %7.4f\n",time,Altitude,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,Phi,Psi);
 510          //printf("Mcg: %7.4f, Mrp: %7.4f, Maero: %7.4f, Meng: %7.4f, Mgear: %7.4f, Dx_cg: %7.4f, Dz_cg: %7.4f\n\n",M_m_cg,M_m_rp,M_m_aero,M_m_engine,M_m_gear,Dx_cg,Dz_cg);
 511                  fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_true_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
 512                  fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 513         }
 514         printf("%g,%g\n",theta_trim*RAD_TO_DEG,htarget*RAD_TO_DEG);
 515         fclose(out);
 516 }
 517
 518 void do_takeoff(FILE *out)
 519 {
 520         SCALAR htarget,hgain,hdiffgain,elev_trim,elev_trim_save,herr;
 521         SCALAR time,herrprev,herr_diff;
 522
 523         htarget=0;
 524
 525         hgain=1;
 526         hdiffgain=1;
 527         elev_trim=Long_control;
 528         elev_trim_save=elev_trim;
 529
 530
 531         out=fopen("takeoff.out","w");
 532         herr=Q_body-htarget;
 533
 534                 // attempt to maintain zero pitch rate during the roll
 535                 while((V_calibrated_kts < 61) && (time < 30.0))
 536                 {
 537                         // herrprev=herr
 538                         ls_update(1);
 539                         // herr=Q_body-htarget;
 540                         // herr_diff=herr-herrprev;
 541                         // Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
 542                         time+=0.01;
 543                         printf("Time: %7.4f, Vc: %7.4f, Alpha: %7.4f, pelev: %7.4f, qdot: %7.4f, udot: %7.4f, U: %7.4f, W: %7.4f\n",time,V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,U_dot_body,U_body,W_body);
 544 //              printf("Mcg: %7.4f, Mrp: %7.4f, Maero: %7.4f, Meng: %7.4f, Mgear: %7.4f, Dx_cg: %7.4f, Dz_cg: %7.4f\n\n",M_m_cg,M_m_rp,M_m_aero,M_m_engine,M_m_gear,Dx_cg,Dz_cg);
 545 //                      fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
 546                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 547
 548                 }
 549                 //At Vr, ramp in 10% nose up elevator in 0.5 seconds
 550                 elev_trim_save=0;
 551                 printf("At Vr, rotate...\n");
 552                 while((Q_body < 3.0*RAD_TO_DEG) && (time < 30.0))
 553                 {
 554                         Long_control-=0.01;
 555                         ls_update(1);
 556                         printf("Time: %7.4f, Vc: %7.4f, Alpha: %7.4f, pelev: %7.4f, q: %7.4f, cm: %7.4f, U: %7.4f, W: %7.4f\n",time,V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control*100,Q_body*RAD_TO_DEG,cm,U_body,W_body);
 557
 558                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
 559                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 560                         time +=0.01;
 561
 562                 }
 563                 //Maintain 15 degrees theta for the climbout
 564                 htarget=15*DEG_TO_RAD;
 565                 herr=Theta-htarget;
 566                 hgain=10;
 567                 hdiffgain=1;
 568                 elev_trim=Long_control;
 569                 while(time < 30.0)
 570                 {
 571                         herrprev=herr;
 572                         ls_update(1);
 573                         herr=Theta-htarget;
 574                         herr_diff=herr-herrprev;
 575                         Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
 576                         time+=0.01;
 577                         printf("Time: %7.4f, Alt: %7.4f, Speed: %7.4f, Theta: %7.4f\n",time,Altitude,V_calibrated_kts,Theta*RAD_TO_DEG);
 578                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,%20.8f,",time,V_calibrated_kts,Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,Q_body*RAD_TO_DEG,Alpha_dot*RAD_TO_DEG,Q_dot_body*RAD_TO_DEG,Throttle_pct,elevator*RAD_TO_DEG);
 579                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 580                 }
 581                 fclose(out);
 582                 printf("Speed: %7.4f, Alt: %7.4f, Alpha: %7.4f, pelev: %7.4f, q: %7.4f, udot: %7.4f\n",V_true_kts,Altitude,Alpha*RAD_TO_DEG,Long_control,Q_body*RAD_TO_DEG,U_dot_body);
 583                 printf("F_down_total: %7.4f, F_Z_aero: %7.4f, F_X: %7.4f, M_m_cg: %7.4f\n\n",F_down+Mass*Gravity,F_Z_aero,F_X,M_m_cg);
 584
 585
 586
 587
 588 }
 589 */
 590
 591
 592