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
  42
  43
  44 void do_trims(int kmax,FILE *out,InitialConditions IC)
  45 {
  46         int k=0,i,j;
  47         double speed,elevator,cmcl,maxspeed;
  48         out=fopen("trims.out","w");
  49         speed=55;
  50
  51         for(j=0;j<=30;j+=10)
  52         {
  53                 IC.flap_handle=j;
  54                 for(i=1;i<=5;i++)
  55                 {
  56                         switch(i)
  57                         {
  58                                 case 1: IC.weight=1500;IC.cg=0.155;break;
  59                                 case 2: IC.weight=1500;IC.cg=0.364;break;
  60                                 case 3: IC.weight=1950;IC.cg=0.155;break;
  61                                 case 4: IC.weight=2550;IC.cg=0.257;break;
  62                                 case 5: IC.weight=2550;IC.cg=0.364;break;
  63                         }
  64
  65                         speed=40;
  66                         if(j > 0) { maxspeed = 90; }
  67                         else { maxspeed = 170; }
  68                         while(speed <= maxspeed)
  69                         {
  70                            IC.vc=speed;
  71                            Long_control=0;Theta=0;Throttle_pct=0.0;
  72
  73                            k=trim_long(kmax,IC);
  74                            if(Long_control <= 0)
  75                                   elevator=Long_control*28;
  76                            else
  77                                  elevator=Long_control*23;
  78                            if(fabs(CL) > 1E-3)
  79                            {
  80                                         cmcl=cm / CL;
  81                            }
  82                            if(k < kmax)
  83                            {
  84                                         fprintf(out,"%g,%g,%g,%g,%g,%d",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Flap_Position,k);
  85                                         fprintf(out,",%g,%g,%g,%g,%g\n",CL,cm,cmcl,Weight,Cg);
  86         /*                              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);
  87          */                }
  88                    else
  89                            {
  90                                  printf("kmax exceeded at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_calibrated_kts,Weight,Cg,Flap_Position);
  91                                  printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
  92                  printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
  93                            }
  94                            speed+=10;
  95                         }
  96         }
  97         }
  98         fclose(out);
  99 }
 100
 101 void find_trim_stall(int kmax,FILE *out,InitialConditions IC)
 102 {
 103         int k=0,i,j;
 104         int failf;
 105         char axis[10];
 106         double speed,elevator,cmcl,speed_inc,lastgood;
 107         out=fopen("trim_stall.summary","w");
 108         speed=90;
 109         speed_inc=10;
 110         //failf=malloc(sizeof(int));
 111
 112         for(j=0;j<=30;j+=10)
 113         {
 114                 IC.flap_handle=j;
 115                 for(i=1;i<=6;i++)
 116                 {
 117                         switch(i)
 118                         {
 119                                 case 1: IC.weight=1500;IC.cg=0.155;break;
 120                                 case 2: IC.weight=1500;IC.cg=0.364;break;
 121                                 case 3: IC.weight=2400;IC.cg=0.155;break;
 122                                 case 4: IC.weight=2400;IC.cg=0.364;break;
 123                                 case 5: IC.weight=2550;IC.cg=0.257;break;
 124                                 case 6: IC.weight=2550;IC.cg=0.364;break;
 125                         }
 126
 127                         speed=90;
 128                         speed_inc=10;
 129                         while(speed_inc >= 0.5)
 130                         {
 131                            IC.vc=speed;
 132                            Long_control=0;Theta=0;Throttle_pct=0.0;
 133                            failf=trim_longfr(kmax,IC);
 134                            if(Long_control <= 0)
 135                                   elevator=Long_control*28;
 136                            else
 137                                  elevator=Long_control*23;
 138                            if(fabs(CL) > 1E-3)
 139                            {
 140                                         cmcl=cm / CL;
 141                            }
 142                            if(failf == 0)
 143                            {
 144                                         lastgood=speed;
 145                                         axis[0]='\0';
 146                                         //fprintf(out,"%g,%g,%g,%g,%g,%d",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Flap_Position,k);
 147                                         //fprintf(out,",%g,%g,%g,%g,%g\n",CL,cm,cmcl,Weight,Cg);
 148         /*                              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);
 149          */                }
 150                    else
 151                            {
 152                                  printf("trim failed at: %g knots, %g lbs, %g %%MAC, Flaps: %g\n",V_calibrated_kts,Weight,Cg,Flap_Position);
 153                                  printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
 154                  printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha*RAD_TO_DEG,Throttle_pct,Long_control);
 155                                  printf("Speed increment: %g\n",speed_inc);
 156                                  speed+=speed_inc;
 157                                  speed_inc/=2;
 158                            }
 159                            speed-=speed_inc;
 160
 161
 162                         }
 163                         printf("failf %d\n",failf);
 164                         if(failf == 1)
 165                            strcpy(axis,"lift");
 166                         else if(failf == 2)
 167                            strcpy(axis,"thrust");
 168                         else if(failf == 3)
 169                            strcpy(axis,"pitch");
 170                         fprintf(out,"Last good speed: %g, Flaps: %g, Weight: %g, CG: %g, failed axis: %s\n",lastgood,Flap_handle,Weight,Cg,axis);
 171
 172
 173         }
 174         }
 175         fclose(out);
 176         //free(failf);
 177 }
 178
 179
 180 // Initialize the LaRCsim flight model, dt is the time increment for
 181 // each subsequent iteration through the EOM
 182 int fgLaRCsimInit(double dt) {
 183     ls_toplevel_init(dt);
 184
 185     return(1);
 186 }
 187
 188
 189
 190 // Run an iteration of the EOM (equations of motion)
 191 int main(int argc, char *argv[]) {
 192
 193
 194         double save_alt = 0.0;
 195     int multiloop=1,k=0,i,j;
 196         double time=0,elev_trim,elev_trim_save,elevator,speed,cmcl;
 197         FILE *out;
 198         double hgain,hdiffgain,herr,herrprev,herr_diff,htarget;
 199         double lastVt,vtdots,vtdott;
 200         InitialConditions IC;
 201     SCALAR *control[7];
 202         SCALAR *state[7];
 203         float old_state,effectiveness,tol,delta_state,lctrim;
 204         float newcm,lastcm,cmalpha;
 205
 206     if(argc < 6)
 207         {
 208             printf("Need args: $c172 speed alt alpha elev throttle\n");
 209                 exit(1);
 210         }
 211         initIC(&IC);
 212
 213         IC.latitude=47.5299892; //BFI
 214         IC.longitude=122.3019561;
 215         Runway_altitude =   18.0;
 216         IC.altitude=strtod(argv[2],NULL);
 217         IC.vc=strtod(argv[1],NULL);
 218         IC.alpha=0;
 219         IC.beta=0;
 220         IC.gamma=strtod(argv[3],NULL);
 221         IC.use_gamma_tmg=1;
 222         IC.phi=0;
 223         IC.psi=10;
 224         IC.weight=2300;
 225         IC.cg=0.25;
 226         IC.flap_handle=0;
 227         IC.long_control=strtod(argv[4],NULL);
 228         IC.rudder_pedal=0;
 229
 230         printf("IC.vc: %g\n",IC.vc);
 231         ls_ForceAltitude(IC.altitude);
 232     fgLaRCsimInit(0.01);
 233         printf("\nLong_control: %g\n\n",Long_control);
 234
 235
 236         IC.cg=0.155;
 237     IC.alpha=-5;
 238         setIC(IC);ls_loop(0.0,-1);
 239         newcm=CLwbh*(IC.cg - 0.557);
 240         lastcm=newcm;
 241         out=fopen("cmcl.out","w");
 242         while(IC.alpha < 22)
 243         {
 244                 IC.alpha+=1;
 245                 setIC(IC);ls_loop(0.0,-1);
 246                 newcm=CLwbh*(IC.cg - 0.557);
 247                 cmalpha=newcm-lastcm;
 248                 printf("alpha: %4.0f, CL: %5.2f, Cm: %5.2f, Cma: %7.4f\n",Alpha*RAD_TO_DEG,CLwbh,newcm,cmalpha);
 249                 fprintf(out,"%g %g\n",newcm,CLwbh);
 250                 lastcm=newcm;
 251         }
 252         fclose(out);
 253         /* find_trim_stall(200,out,IC);
 254
 255     IC.vc=120;
 256         IC.altitude=8000;
 257         IC.weight=2300;
 258         IC.cg=0.25;
 259         IC.flap_handle=0;
 260
 261
 262     setIC(IC);
 263         printIC(IC);
 264         k=trim_long(100,IC);
 265
 266         printf("Flap_handle: %g, Flap_Position: %g\n",Flap_handle,Flap_Position);
 267         printf("k: %d, %g knots, %g lbs, %g %%MAC\n",k,V_calibrated_kts,Weight,Cg);
 268         printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
 269     printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha,Throttle_pct,Long_control);
 270
 271         printf("Cme: %g, elevator: %g, Cmde: %g\n",elevator*Cmde,elevator,Cmde);
 272
 273     IC.cg=0.155;
 274     setIC(IC);
 275         k=trim_long(100,IC);
 276
 277         printf("Flap_handle: %g, Flap_Position: %g\n",Flap_handle,Flap_Position);
 278         printf("k: %d, %g knots, %g lbs, %g %%MAC\n",k,V_calibrated_kts,Weight,Cg);
 279         printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
 280     printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha,Throttle_pct,Long_control);
 281
 282         printf("Cme: %g, elevator: %g, Cmde: %g\n",elevator*Cmde,elevator,Cmde);
 283
 284         IC.cg=0.364;
 285         setIC(IC);
 286         k=trim_long(100,IC);
 287
 288         printf("Flap_handle: %g, Flap_Position: %g\n",Flap_handle,Flap_Position);
 289         printf("k: %d, %g knots, %g lbs, %g %%MAC\n",k,V_calibrated_kts,Weight,Cg);
 290         printf("wdot: %g, udot: %g, qdot: %g\n",W_dot_body,U_dot_body,Q_dot_body);
 291     printf("Alpha: %g, Throttle_pct: %g, Long_control: %g\n\n",Alpha,Throttle_pct,Long_control);
 292
 293         printf("Cme: %g, elevator: %g, Cmde: %g\n",elevator*Cmde,elevator,Cmde);
 294          */
 295
 296
 297
 298
 299
 300         /* do_trims(400,out,IC); */
 301
 302         /* ls_loop(0.0,-1);
 303
 304         control[1]=&IC.long_control;
 305         control[2]=&IC.throttle;
 306         control[3]=&IC.alpha;
 307         control[4]=&IC.beta;
 308         control[5]=&IC.phi;
 309         control[6]=&IC.lat_control;
 310
 311         state[1]=&Q_dot_body;state[2]=&U_dot_body;state[3]=&W_dot_body;
 312         state[4]=&R_dot_body;state[5]=&V_dot_body;state[6]=&P_dot_body;
 313
 314
 315         for(i=1;i<=6;i++)
 316         {
 317                 old_state=*state[i];
 318             tol=1E-4;
 319                 for(j=1;j<=6;j++)
 320                 {
 321                         *control[j]+=0.1;
 322                         setIC(IC);
 323                         ls_loop(0.0,-1);
 324                         delta_state=*state[i]-old_state;
 325                         effectiveness=(delta_state)/ 0.1;
 326                         if(delta_state < tol)
 327                                 effectiveness = 0;
 328                         printf("%8.4f,",delta_state);
 329                         *control[j]-=0.1;
 330
 331                 }
 332                 printf("\n");
 333                 setIC(IC);
 334                 ls_loop(0.0,-1);
 335         }                */
 336
 337             return 1;
 338 }
 339
 340 /*
 341 void do_stick_pull(int kmax, SCALAR tmax,FILE *out,InitialConditions IC)
 342 {
 343
 344         SCALAR htarget,hgain,hdiffgain,herr,herr_diff,herrprev;
 345         SCALAR theta_trim,elev_trim,time;
 346         int k;
 347         k=trim_long(kmax,IC);
 348         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);
 349
 350
 351         htarget=0;
 352
 353         hgain=1;
 354         hdiffgain=1;
 355         elev_trim=Long_control;
 356         out=fopen("stick_pull.out","w");
 357         herr=Q_body-htarget;
 358
 359         //fly steady-level for 2 seconds, well, zero pitch rate anyway
 360         while(time < 2.0)
 361         {
 362                  herrprev=herr;
 363                  ls_update(1);
 364                  herr=Q_body-htarget;
 365                  herr_diff=herr-herrprev;
 366                  Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
 367                  time+=0.01;
 368                  //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);
 369          //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);
 370                  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);
 371                  fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 372         }
 373
 374         //begin untrimmed climb at theta_trim + 2 degrees
 375         hgain=4;
 376         hdiffgain=2;
 377         theta_trim=Theta;
 378         htarget=theta_trim;
 379         herr=Theta-htarget;
 380         while(time < tmax)
 381         {
 382                 //ramp in the target theta
 383                 if(htarget < (theta_trim + 2*DEG_TO_RAD))
 384                 {
 385                         htarget+= 0.01*DEG_TO_RAD;
 386                 }
 387                 herrprev=herr;
 388                  ls_update(1);
 389                  herr=Theta-htarget;
 390                  herr_diff=herr-herrprev;
 391                  Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
 392                  time+=0.01;
 393                  //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);
 394          //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);
 395                  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);
 396                  fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 397         }
 398         printf("%g,%g\n",theta_trim*RAD_TO_DEG,htarget*RAD_TO_DEG);
 399         fclose(out);
 400 }
 401
 402 void do_takeoff(FILE *out)
 403 {
 404         SCALAR htarget,hgain,hdiffgain,elev_trim,elev_trim_save,herr;
 405         SCALAR time,herrprev,herr_diff;
 406
 407         htarget=0;
 408
 409         hgain=1;
 410         hdiffgain=1;
 411         elev_trim=Long_control;
 412         elev_trim_save=elev_trim;
 413
 414
 415         out=fopen("takeoff.out","w");
 416         herr=Q_body-htarget;
 417
 418                 // attempt to maintain zero pitch rate during the roll
 419                 while((V_calibrated_kts < 61) && (time < 30.0))
 420                 {
 421                         // herrprev=herr
 422                         ls_update(1);
 423                         // herr=Q_body-htarget;
 424                         // herr_diff=herr-herrprev;
 425                         // Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
 426                         time+=0.01;
 427                         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);
 428 //              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);
 429 //                      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);
 430                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 431
 432                 }
 433                 //At Vr, ramp in 10% nose up elevator in 0.5 seconds
 434                 elev_trim_save=0;
 435                 printf("At Vr, rotate...\n");
 436                 while((Q_body < 3.0*RAD_TO_DEG) && (time < 30.0))
 437                 {
 438                         Long_control-=0.01;
 439                         ls_update(1);
 440                         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);
 441
 442                         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);
 443                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 444                         time +=0.01;
 445
 446                 }
 447                 //Maintain 15 degrees theta for the climbout
 448                 htarget=15*DEG_TO_RAD;
 449                 herr=Theta-htarget;
 450                 hgain=10;
 451                 hdiffgain=1;
 452                 elev_trim=Long_control;
 453                 while(time < 30.0)
 454                 {
 455                         herrprev=herr;
 456                         ls_update(1);
 457                         herr=Theta-htarget;
 458                         herr_diff=herr-herrprev;
 459                         Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
 460                         time+=0.01;
 461                         printf("Time: %7.4f, Alt: %7.4f, Speed: %7.4f, Theta: %7.4f\n",time,Altitude,V_calibrated_kts,Theta*RAD_TO_DEG);
 462                         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);
 463                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 464                 }
 465                 fclose(out);
 466                 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);
 467                 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);
 468
 469
 470
 471
 472 }
 473 */
 474
 475
 476