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
  35 #include <math.h>
  36 #include <stdlib.h>
  37 #include <stdio.h>
  38
  39
  40 //simple "one-at-a-time" longitudinal trimming routine
  41 typedef struct
  42 {
  43         double latitude,longitude,altitude;
  44         double vc,alpha,beta,gamma;
  45         double theta,phi,psi;
  46         double weight,cg;
  47         int use_gamma_tmg;
  48 }InitialConditions;
  49
  50 // Units for setIC
  51 // vc       knots (calibrated airspeed, close to indicated)
  52 // altitude ft
  53 // all angles in degrees
  54 // weight lbs
  55 // cg %MAC
  56 // if use_gamma_tmg =1 then theta will be computed
  57 // from theta=alpha+gamma and the value given will
  58 // be ignored. Otherwise gamma is computed from
  59 // gamma=theta-alpha
  60 void setIC(InitialConditions IC)
  61 {
  62         SCALAR vtfps,u,v,w,vt_east;
  63         SCALAR vnu,vnv,vnw,vteu,vtev,vtew,vdu,vdv,vdw;
  64         SCALAR alphar,betar,thetar,phir,psir,gammar;
  65         SCALAR sigma,ps,Ts,a;
  66
  67         Mass=IC.weight*INVG;
  68         Dx_cg=(IC.cg-0.25)*4.9;
  69
  70         Latitude=IC.latitude*DEG_TO_RAD;
  71         Longitude=IC.longitude*DEG_TO_RAD;
  72         Altitude=IC.altitude;
  73         ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
  74
  75         ls_atmos(IC.altitude,&sigma,&a,&Ts,&ps);
  76         vtfps=sqrt(1/sigma*IC.vc*IC.vc)*1.68781;
  77         alphar=IC.alpha*DEG_TO_RAD;
  78         betar=IC.beta*DEG_TO_RAD;
  79         gammar=IC.gamma*DEG_TO_RAD;
  80
  81
  82         phir=IC.phi*DEG_TO_RAD;
  83         psir=IC.psi*DEG_TO_RAD;
  84
  85         if(IC.use_gamma_tmg == 1)
  86         {
  87            thetar=alphar+gammar;
  88         }
  89         else
  90         {
  91            thetar=IC.theta*DEG_TO_RAD;
  92            gammar=thetar-alphar;
  93         }
  94
  95         u=vtfps*cos(alphar)*cos(betar);
  96         v=vtfps*sin(betar);
  97         w=vtfps*sin(alphar)*cos(betar);
  98
  99         vnu=u*cos(thetar)*cos(psir);
 100         vnv=v*(-sin(psir)*cos(phir)+sin(phir)*sin(thetar)*cos(psir));
 101         vnw=w*(sin(phir)*sin(psir)+cos(phir)*sin(thetar)*cos(psir));
 102
 103         V_north=vnu+vnv+vnw;
 104
 105         vteu=u*cos(thetar)*sin(psir);
 106         vtev=v*(cos(phir)*cos(psir)+sin(phir)*sin(thetar)*sin(psir));
 107         vtew=w*(-sin(phir)*cos(psir)+cos(phir)*sin(thetar)*sin(psir));
 108
 109         vt_east=vteu+vtev+vtew;
 110         V_east=vt_east+ OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 111
 112     vdu=u*-sin(thetar);
 113         vdv=v*cos(thetar)*sin(phir);
 114         vdw=w*cos(thetar)*cos(phir);
 115
 116         V_down=vdu+vdv+vdw;
 117
 118         Theta=thetar;
 119         Phi=phir;
 120         Psi=psir;
 121
 122 }
 123
 124
 125 int trim_long(int kmax, InitialConditions IC)
 126 {
 127         double elevator,alpha;
 128         double tol=1E-3;
 129         double a_tol=tol/10;
 130         double alpha_step=0.001;
 131         int k=0,i,j=0,jmax=10,sum=0;
 132         ls_loop(0.0,-1);
 133         do{
 134                 //printf("k: %d\n",k);
 135                 while((fabs(W_dot_body) > tol) && (j < jmax))
 136                 {
 137
 138                         IC.alpha+=W_dot_body*0.05;
 139                         if((IC.alpha < -5) || (IC.alpha > 21))
 140                            j=jmax;
 141                         setIC(IC);
 142             ls_loop(0.0,-1);
 143 /*                      printf("IC.alpha: %g, Alpha: %g, wdot: %g\n",IC.alpha,Alpha*RAD_TO_DEG,W_dot_body);
 144  */                     j++;
 145                 }
 146                 sum+=j;
 147 /*              printf("\tTheta: %7.4f, Alpha: %7.4f, wdot: %10.6f, j: %d\n",Theta*RAD_TO_DEG,Alpha*RAD_TO_DEG,W_dot_body,j);
 148  */             j=0;
 149                 while((fabs(U_dot_body) > tol) && (j < jmax))
 150                 {
 151
 152                         Throttle_pct-=U_dot_body*0.005;
 153                         if((Throttle_pct < 0) || (Throttle_pct > 1))
 154                                 Throttle_pct=0.2;
 155                         setIC(IC);
 156             ls_loop(0.0,-1);
 157                         j++;
 158                 }
 159                 sum+=j;
 160 /*              printf("\tThrottle_pct: %7.4f, udot: %10.6f, j: %d\n",Throttle_pct,U_dot_body,j);
 161  */        j=0;
 162                 while((fabs(Q_dot_body) > a_tol) && (j < jmax))
 163                 {
 164
 165             Long_control+=Q_dot_body*0.001;
 166                         if((Long_control < -1) || (Long_control > 1))
 167                                 j=jmax;
 168             setIC(IC);
 169                         ls_loop(0.0,-1);
 170                         j++;
 171                 }
 172                 sum+=j;
 173                 if(Long_control >= 0)
 174                         elevator=Long_control*23;
 175                 else
 176                         elevator=Long_control*28;
 177 /*              printf("\televator: %7.4f, qdot: %10.6f, j: %d\n",elevator,Q_dot_body,j);
 178  */        k++;j=0;
 179     }while(((fabs(W_dot_body) > tol) || (fabs(U_dot_body) > tol) || (fabs(Q_dot_body) > tol)) && (k < kmax));
 180         /* printf("Total Iterations: %d\n",sum); */
 181         return k;
 182 }
 183
 184 int trim_ground(int kmax, InitialConditions IC)
 185 {
 186         double elevator,alpha,qdot_prev,alt_prev,step;
 187         double tol=1E-3;
 188         double a_tol=tol/10;
 189         double alpha_step=0.001;
 190         int k=0,i,j=0,jmax=40,sum=0,m=0;
 191         Throttle_pct=0;
 192         Brake_pct=1;
 193         Theta=5*DEG_TO_RAD;
 194         IC.altitude=Runway_altitude;
 195         printf("udot: %g\n",U_dot_body);
 196         setIC(IC);
 197         printf("Altitude: %g, Runway_altitude: %g\n",Altitude,Runway_altitude);
 198         qdot_prev=1.0E6;
 199
 200         ls_loop(0.0,-1);
 201
 202         do{
 203                 //printf("k: %d\n",k);
 204                 step=1;
 205             printf("IC.altitude: %g, Altitude: %g, Runway_altitude: %g,wdot: %g,F_Z_gear: %g, M_m_gear: %g,F_Z: %g\n",IC.altitude,Altitude,Runway_altitude,W_dot_body,F_Z_gear,M_m_gear,F_Z);
 206
 207                 m=0;
 208                 while((fabs(W_dot_body) > tol) && (m < 10))
 209                 {
 210
 211                         j=0;
 212
 213                         do{
 214                                 alt_prev=IC.altitude;
 215                                 IC.altitude+=step;
 216                                 setIC(IC);
 217                 ls_loop(0.0,-1);
 218                                 printf("IC.altitude: %g, Altitude: %g, Runway_altitude: %g,wdot: %g,F_Z: %g\n",IC.altitude,Altitude,Runway_altitude,W_dot_body,F_Z);
 219                                 j++;
 220                         }while((W_dot_body < 0) && (j < jmax));
 221                         IC.altitude-=step;
 222                         step/=10;
 223                         printf("step: %g\n",step);
 224                         m++;
 225
 226                 }
 227                 sum+=j;
 228         printf("IC.altitude: %g, Altitude: %g, Runway_altitude: %g,wdot: %g,F_Z_gear: %g, M_m_gear: %g,F_Z: %g\n",IC.altitude,Altitude,Runway_altitude,W_dot_body,F_Z_gear,M_m_gear,F_Z);
 229
 230         j=0;
 231
 232                 while((Q_dot_body <= qdot_prev) && (j < jmax))
 233                 {
 234
 235
 236                         qdot_prev=Q_dot_body;
 237                         IC.theta+=Q_dot_body;
 238             setIC(IC);
 239                         ls_loop(0.0,-1);
 240                         j++;
 241
 242                         printf("\tTheta: %7.4f, qdot: %10.6f, qdot_prev: %10.6f, j: %d\n",Theta*RAD_TO_DEG,Q_dot_body,qdot_prev,j);
 243                 }
 244                 IC.theta-=qdot_prev;
 245                 sum+=j;
 246
 247                 printf("\tTheta: %7.4f, qdot: %10.6f, W_dot_body: %g\n",Theta,Q_dot_body,W_dot_body);
 248         j=0;
 249                 if(W_dot_body > tol)
 250                 {
 251                         step=1;
 252                         while((W_dot_body > 0) && (j <jmax))
 253                         {
 254                                 IC.altitude-=step;
 255                                 setIC(IC);
 256                                 ls_loop(0.0,-1);
 257                                 j++;
 258                         }
 259                 }
 260                 k++;j=0;
 261     }while(((fabs(W_dot_body) > tol) || (fabs(Q_dot_body) > tol)) && (k < kmax));
 262         printf("Total Iterations: %d\n",sum);
 263         return k;
 264 }
 265 void do_trims(int kmax,FILE *out,InitialConditions IC)
 266 {
 267         int k=0,i;
 268         double speed,elevator,cmcl;
 269         out=fopen("trims.out","w");
 270         speed=55;
 271
 272         for(i=1;i<=5;i++)
 273         {
 274                 switch(i)
 275                 {
 276                         case 1: IC.weight=1500;IC.cg=0.155;break;
 277                         case 2: IC.weight=1500;IC.cg=0.364;break;
 278                         case 3: IC.weight=1950;IC.cg=0.155;break;
 279                         case 4: IC.weight=2550;IC.cg=0.257;break;
 280                         case 5: IC.weight=2550;IC.cg=0.364;break;
 281                 }
 282
 283                 speed=50;
 284                 while(speed <= 150)
 285                 {
 286                    IC.vc=speed;
 287                    Long_control=0;Theta=0;Throttle_pct=0.0;
 288
 289                    k=trim_long(kmax,IC);
 290                    if(Long_control <= 0)
 291                           elevator=Long_control*28;
 292                    else
 293                          elevator=Long_control*23;
 294                    if(fabs(CL) > 1E-3)
 295                    {
 296                                 cmcl=cm / CL;
 297                    }
 298                    if(k < kmax)
 299                    {
 300                                 fprintf(out,"%g,%g,%g,%g,%g,%d",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Gamma_vert_rad,k);
 301                                 fprintf(out,",%g,%g,%g,%g,%g\n",CL,cm,cmcl,Weight,Cg);
 302 /*                              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);
 303  */                }
 304            else
 305                    {
 306                          printf("kmax exceeded at: %g knots, %g lbs, %g %%MAC\n",V_calibrated_kts,Weight,Cg);
 307                          printf("wdot: %g, udot: %g, qdot: %g\n\n",W_dot_body,U_dot_body,Q_dot_body);
 308
 309                    }
 310                    speed+=10;
 311                 }
 312     }
 313         fclose(out);
 314 }
 315
 316 void do_stick_pull(int kmax, SCALAR tmax,FILE *out,InitialConditions IC)
 317 {
 318
 319         SCALAR htarget,hgain,hdiffgain,herr,herr_diff,herrprev;
 320         SCALAR theta_trim,elev_trim,time;
 321         int k;
 322         k=trim_long(kmax,IC);
 323         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);
 324
 325
 326         htarget=0;
 327
 328         hgain=1;
 329         hdiffgain=1;
 330         elev_trim=Long_control;
 331         out=fopen("stick_pull.out","w");
 332         herr=Q_body-htarget;
 333
 334         //fly steady-level for 2 seconds, well, zero pitch rate anyway
 335         while(time < 2.0)
 336         {
 337                  herrprev=herr;
 338                  ls_update(1);
 339                  herr=Q_body-htarget;
 340                  herr_diff=herr-herrprev;
 341                  Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
 342                  time+=0.01;
 343 /*               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);
 344          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);
 345  */              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);
 346                  fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 347         }
 348
 349         //begin untrimmed climb at theta_trim + 2 degrees
 350         hgain=4;
 351         hdiffgain=2;
 352         theta_trim=Theta;
 353         htarget=theta_trim;
 354         herr=Theta-htarget;
 355         while(time < tmax)
 356         {
 357                 //ramp in the target theta
 358                 if(htarget < (theta_trim + 2*DEG_TO_RAD))
 359                 {
 360                         htarget+= 0.01*DEG_TO_RAD;
 361                 }
 362                 herrprev=herr;
 363                  ls_update(1);
 364                  herr=Theta-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         printf("%g,%g\n",theta_trim*RAD_TO_DEG,htarget*RAD_TO_DEG);
 374         fclose(out);
 375 }
 376
 377 void do_takeoff(FILE *out)
 378 {
 379         SCALAR htarget,hgain,hdiffgain,elev_trim,elev_trim_save,herr;
 380         SCALAR time,herrprev,herr_diff;
 381
 382         htarget=0;
 383
 384         hgain=1;
 385         hdiffgain=1;
 386         elev_trim=Long_control;
 387         elev_trim_save=elev_trim;
 388
 389
 390         out=fopen("takeoff.out","w");
 391         herr=Q_body-htarget;
 392
 393                 //attempt to maintain zero pitch rate during the roll
 394                 while((V_calibrated_kts < 61) && (time < 30.0))
 395                 {
 396                         /* herrprev=herr;*/
 397                         ls_update(1);
 398                         /*herr=Q_body-htarget;
 399                         herr_diff=herr-herrprev;
 400                         Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff); */
 401                         time+=0.01;
 402                         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);
 403 //              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);
 404 //                      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);
 405                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 406
 407                 }
 408                 //At Vr, ramp in 10% nose up elevator in 0.5 seconds
 409                 elev_trim_save=0;
 410                 printf("At Vr, rotate...\n");
 411                 while((Q_body < 3.0*RAD_TO_DEG) && (time < 30.0))
 412                 {
 413                         Long_control-=0.01;
 414                         ls_update(1);
 415                         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);
 416
 417                         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);
 418                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 419                         time +=0.01;
 420
 421                 }
 422                 //Maintain 15 degrees theta for the climbout
 423                 htarget=15*DEG_TO_RAD;
 424                 herr=Theta-htarget;
 425                 hgain=10;
 426                 hdiffgain=1;
 427                 elev_trim=Long_control;
 428                 while(time < 30.0)
 429                 {
 430                         herrprev=herr;
 431                         ls_update(1);
 432                         herr=Theta-htarget;
 433                         herr_diff=herr-herrprev;
 434                         Long_control=elev_trim+(hgain*herr + hdiffgain*herr_diff);
 435                         time+=0.01;
 436                         printf("Time: %7.4f, Alt: %7.4f, Speed: %7.4f, Theta: %7.4f\n",time,Altitude,V_calibrated_kts,Theta*RAD_TO_DEG);
 437                         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);
 438                         fprintf(out,"%20.8f,%20.8f,%20.8f,%20.8f,%20.8f\n",CL,CLwbh,cm,cd,Altitude);
 439                 }
 440                 fclose(out);
 441                 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);
 442                 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);
 443
 444
 445
 446
 447 }
 448
 449 // Initialize the LaRCsim flight model, dt is the time increment for
 450 // each subsequent iteration through the EOM
 451 int fgLaRCsimInit(double dt) {
 452     ls_toplevel_init(dt);
 453
 454     return(1);
 455 }
 456
 457
 458
 459 // Run an iteration of the EOM (equations of motion)
 460 int main(int argc, char *argv[]) {
 461
 462
 463         double save_alt = 0.0;
 464     int multiloop=1,k=0,i;
 465         double time=0,elev_trim,elev_trim_save,elevator,speed,cmcl;
 466         FILE *out;
 467         double hgain,hdiffgain,herr,herrprev,herr_diff,htarget;
 468         InitialConditions IC;
 469
 470     if(argc < 6)
 471         {
 472             printf("Need args: $c172 speed alt alpha elev throttle\n");
 473                 exit(1);
 474         }
 475
 476         IC.latitude=47.5299892; //BFI
 477         IC.longitude=122.3019561;
 478         Runway_altitude =   18.0;
 479         IC.altitude=strtod(argv[2],NULL);
 480         IC.vc=strtod(argv[1],NULL);
 481         IC.alpha=10;
 482         IC.beta=0;
 483         IC.theta=strtod(argv[3],NULL);
 484         IC.use_gamma_tmg=0;
 485         IC.phi=0;
 486         IC.psi=0;
 487         IC.weight=1500;
 488         IC.cg=0.155;
 489         Long_control=strtod(argv[4],NULL);
 490     setIC(IC);
 491         printf("Out setIC\n");
 492         ls_ForceAltitude(IC.altitude);
 493     fgLaRCsimInit(0.01);
 494
 495         while(IC.alpha < 30.0)
 496         {
 497                 setIC(IC);
 498                 ls_loop(0.0,-1);
 499                 printf("CL: %g ,Alpha: %g\n",CL,IC.alpha);
 500                 IC.alpha+=1.0;
 501         }
 502
 503         /*trim_ground(10,IC);*/
 504         /* printf("%g,%g\n",Theta,Gamma_vert_rad);
 505         printf("trim_long():\n");
 506         k=trim_long(200,IC);
 507         Throttle_pct=Throttle_pct-0.2;
 508         printf("%g,%g\n",Theta,Gamma_vert_rad);
 509         out=fopen("dive.out","w");
 510         time=0;
 511         while(time < 30.0)
 512         {
 513                         ls_update(1);
 514
 515                         cmcl=cm/CL;
 516                         fprintf(out,"%g,%g,%g,%g,%g,%d",V_calibrated_kts,Alpha*RAD_TO_DEG,Long_control,Throttle_pct,Gamma_vert_rad,k);
 517                         fprintf(out,",%g,%g,%g\n",CL,cm,cmcl);
 518                         time+=0.01;
 519     }
 520         fclose(out);
 521         printf("V_rel_wind: %8.2f, Alpha: %8.2f, Beta: %8.2f\n",V_rel_wind,Alpha*RAD_TO_DEG,Beta*RAD_TO_DEG);
 522         printf("Theta: %8.2f, Gamma: %8.2f, Alpha_tmg: %8.2f\n",Theta*RAD_TO_DEG,Gamma_vert_rad*RAD_TO_DEG,Theta*RAD_TO_DEG-Gamma_vert_rad*RAD_TO_DEG);
 523         printf("V_north: %8.2f, V_east_rel_ground: %8.2f, V_east: %8.2f, V_down: %8.2f\n",V_north,V_east_rel_ground,V_east,V_down);
 524         printf("Long_control:  %8.2f, Throttle_pct: %8.2f\n",Long_control,Throttle_pct);
 525         printf("k: %d, udot: %8.4f, wdot: %8.4f, qdot: %8.5f\n",k,U_dot_body,W_dot_body,Q_dot_body);
 526
 527         printf("\nls_update():\n");
 528         ls_update(1);
 529         printf("V_rel_wind: %8.2f, Alpha: %8.2f, Beta: %8.2f\n",V_rel_wind,Alpha*RAD_TO_DEG,Beta*RAD_TO_DEG);
 530         printf("Theta: %8.2f, Gamma: %8.2f, Alpha_tmg: %8.2f\n",Theta*RAD_TO_DEG,Gamma_vert_rad*RAD_TO_DEG,Theta*RAD_TO_DEG-Gamma_vert_rad*RAD_TO_DEG);
 531  */
 532
 533     /* Inform LaRCsim of the local terrain altitude */
 534
 535
 536
 537     return 1;
 538 }
 539
 540
 541 /*// Convert from the FGInterface struct to the LaRCsim generic_ struct
 542 int FGInterface_2_LaRCsim (FGInterface& f) {
 543
 544     Mass =      f.get_Mass();
 545     I_xx =      f.get_I_xx();
 546     I_yy =      f.get_I_yy();
 547     I_zz =      f.get_I_zz();
 548     I_xz =      f.get_I_xz();
 549     // Dx_pilot =  f.get_Dx_pilot();
 550     // Dy_pilot =  f.get_Dy_pilot();
 551     // Dz_pilot =  f.get_Dz_pilot();
 552     Dx_cg =     f.get_Dx_cg();
 553     Dy_cg =     f.get_Dy_cg();
 554     Dz_cg =     f.get_Dz_cg();
 555     // F_X =       f.get_F_X();
 556     // F_Y =       f.get_F_Y();
 557     // F_Z =       f.get_F_Z();
 558     // F_north =   f.get_F_north();
 559     // F_east =    f.get_F_east();
 560     // F_down =    f.get_F_down();
 561     // F_X_aero =  f.get_F_X_aero();
 562     // F_Y_aero =  f.get_F_Y_aero();
 563     // F_Z_aero =  f.get_F_Z_aero();
 564     // F_X_engine =        f.get_F_X_engine();
 565     // F_Y_engine =        f.get_F_Y_engine();
 566     // F_Z_engine =        f.get_F_Z_engine();
 567     // F_X_gear =  f.get_F_X_gear();
 568     // F_Y_gear =  f.get_F_Y_gear();
 569     // F_Z_gear =  f.get_F_Z_gear();
 570     // M_l_rp =    f.get_M_l_rp();
 571     // M_m_rp =    f.get_M_m_rp();
 572     // M_n_rp =    f.get_M_n_rp();
 573     // M_l_cg =    f.get_M_l_cg();
 574     // M_m_cg =    f.get_M_m_cg();
 575     // M_n_cg =    f.get_M_n_cg();
 576     // M_l_aero =  f.get_M_l_aero();
 577     // M_m_aero =  f.get_M_m_aero();
 578     // M_n_aero =  f.get_M_n_aero();
 579     // M_l_engine =        f.get_M_l_engine();
 580     // M_m_engine =        f.get_M_m_engine();
 581     // M_n_engine =        f.get_M_n_engine();
 582     // M_l_gear =  f.get_M_l_gear();
 583     // M_m_gear =  f.get_M_m_gear();
 584     // M_n_gear =  f.get_M_n_gear();
 585     // V_dot_north =       f.get_V_dot_north();
 586     // V_dot_east =        f.get_V_dot_east();
 587     // V_dot_down =        f.get_V_dot_down();
 588     // U_dot_body =        f.get_U_dot_body();
 589     // V_dot_body =        f.get_V_dot_body();
 590     // W_dot_body =        f.get_W_dot_body();
 591     // A_X_cg =    f.get_A_X_cg();
 592     // A_Y_cg =    f.get_A_Y_cg();
 593     // A_Z_cg =    f.get_A_Z_cg();
 594     // A_X_pilot = f.get_A_X_pilot();
 595     // A_Y_pilot = f.get_A_Y_pilot();
 596     // A_Z_pilot = f.get_A_Z_pilot();
 597     // N_X_cg =    f.get_N_X_cg();
 598     // N_Y_cg =    f.get_N_Y_cg();
 599     // N_Z_cg =    f.get_N_Z_cg();
 600     // N_X_pilot = f.get_N_X_pilot();
 601     // N_Y_pilot = f.get_N_Y_pilot();
 602     // N_Z_pilot = f.get_N_Z_pilot();
 603     // P_dot_body =        f.get_P_dot_body();
 604     // Q_dot_body =        f.get_Q_dot_body();
 605     // R_dot_body =        f.get_R_dot_body();
 606     V_north =   f.get_V_north();
 607     V_east =    f.get_V_east();
 608     V_down =    f.get_V_down();
 609     // V_north_rel_ground =        f.get_V_north_rel_ground();
 610     // V_east_rel_ground = f.get_V_east_rel_ground();
 611     // V_down_rel_ground = f.get_V_down_rel_ground();
 612     // V_north_airmass =   f.get_V_north_airmass();
 613     // V_east_airmass =    f.get_V_east_airmass();
 614     // V_down_airmass =    f.get_V_down_airmass();
 615     // V_north_rel_airmass =       f.get_V_north_rel_airmass();
 616     // V_east_rel_airmass =        f.get_V_east_rel_airmass();
 617     // V_down_rel_airmass =        f.get_V_down_rel_airmass();
 618     // U_gust =    f.get_U_gust();
 619     // V_gust =    f.get_V_gust();
 620     // W_gust =    f.get_W_gust();
 621     // U_body =    f.get_U_body();
 622     // V_body =    f.get_V_body();
 623     // W_body =    f.get_W_body();
 624     // V_rel_wind =        f.get_V_rel_wind();
 625     // V_true_kts =        f.get_V_true_kts();
 626     // V_rel_ground =      f.get_V_rel_ground();
 627     // V_inertial =        f.get_V_inertial();
 628     // V_ground_speed =    f.get_V_ground_speed();
 629     // V_equiv =   f.get_V_equiv();
 630     // V_equiv_kts =       f.get_V_equiv_kts();
 631     // V_calibrated =      f.get_V_calibrated();
 632     // V_calibrated_kts =  f.get_V_calibrated_kts();
 633     P_body =    f.get_P_body();
 634     Q_body =    f.get_Q_body();
 635     R_body =    f.get_R_body();
 636     // P_local =   f.get_P_local();
 637     // Q_local =   f.get_Q_local();
 638     // R_local =   f.get_R_local();
 639     // P_total =   f.get_P_total();
 640     // Q_total =   f.get_Q_total();
 641     // R_total =   f.get_R_total();
 642     // Phi_dot =   f.get_Phi_dot();
 643     // Theta_dot = f.get_Theta_dot();
 644     // Psi_dot =   f.get_Psi_dot();
 645     // Latitude_dot =      f.get_Latitude_dot();
 646     // Longitude_dot =     f.get_Longitude_dot();
 647     // Radius_dot =        f.get_Radius_dot();
 648     Lat_geocentric =    f.get_Lat_geocentric();
 649     Lon_geocentric =    f.get_Lon_geocentric();
 650     Radius_to_vehicle = f.get_Radius_to_vehicle();
 651     Latitude =  f.get_Latitude();
 652     Longitude = f.get_Longitude();
 653     Altitude =  f.get_Altitude();
 654     Phi =       f.get_Phi();
 655     Theta =     f.get_Theta();
 656     Psi =       f.get_Psi();
 657     // T_local_to_body_11 =        f.get_T_local_to_body_11();
 658     // T_local_to_body_12 =        f.get_T_local_to_body_12();
 659     // T_local_to_body_13 =        f.get_T_local_to_body_13();
 660     // T_local_to_body_21 =        f.get_T_local_to_body_21();
 661     // T_local_to_body_22 =        f.get_T_local_to_body_22();
 662     // T_local_to_body_23 =        f.get_T_local_to_body_23();
 663     // T_local_to_body_31 =        f.get_T_local_to_body_31();
 664     // T_local_to_body_32 =        f.get_T_local_to_body_32();
 665     // T_local_to_body_33 =        f.get_T_local_to_body_33();
 666     // Gravity =   f.get_Gravity();
 667     // Centrifugal_relief =        f.get_Centrifugal_relief();
 668     // Alpha =     f.get_Alpha();
 669     // Beta =      f.get_Beta();
 670     // Alpha_dot = f.get_Alpha_dot();
 671     // Beta_dot =  f.get_Beta_dot();
 672     // Cos_alpha = f.get_Cos_alpha();
 673     // Sin_alpha = f.get_Sin_alpha();
 674     // Cos_beta =  f.get_Cos_beta();
 675     // Sin_beta =  f.get_Sin_beta();
 676     // Cos_phi =   f.get_Cos_phi();
 677     // Sin_phi =   f.get_Sin_phi();
 678     // Cos_theta = f.get_Cos_theta();
 679     // Sin_theta = f.get_Sin_theta();
 680     // Cos_psi =   f.get_Cos_psi();
 681     // Sin_psi =   f.get_Sin_psi();
 682     // Gamma_vert_rad =    f.get_Gamma_vert_rad();
 683     // Gamma_horiz_rad =   f.get_Gamma_horiz_rad();
 684     // Sigma =     f.get_Sigma();
 685     // Density =   f.get_Density();
 686     // V_sound =   f.get_V_sound();
 687     // Mach_number =       f.get_Mach_number();
 688     // Static_pressure =   f.get_Static_pressure();
 689     // Total_pressure =    f.get_Total_pressure();
 690     // Impact_pressure =   f.get_Impact_pressure();
 691     // Dynamic_pressure =  f.get_Dynamic_pressure();
 692     // Static_temperature =        f.get_Static_temperature();
 693     // Total_temperature = f.get_Total_temperature();
 694     Sea_level_radius =  f.get_Sea_level_radius();
 695     Earth_position_angle =      f.get_Earth_position_angle();
 696     Runway_altitude =   f.get_Runway_altitude();
 697     // Runway_latitude =   f.get_Runway_latitude();
 698     // Runway_longitude =  f.get_Runway_longitude();
 699     // Runway_heading =    f.get_Runway_heading();
 700     // Radius_to_rwy =     f.get_Radius_to_rwy();
 701     // D_cg_north_of_rwy = f.get_D_cg_north_of_rwy();
 702     // D_cg_east_of_rwy =  f.get_D_cg_east_of_rwy();
 703     // D_cg_above_rwy =    f.get_D_cg_above_rwy();
 704     // X_cg_rwy =  f.get_X_cg_rwy();
 705     // Y_cg_rwy =  f.get_Y_cg_rwy();
 706     // H_cg_rwy =  f.get_H_cg_rwy();
 707     // D_pilot_north_of_rwy =      f.get_D_pilot_north_of_rwy();
 708     // D_pilot_east_of_rwy =       f.get_D_pilot_east_of_rwy();
 709     // D_pilot_above_rwy = f.get_D_pilot_above_rwy();
 710     // X_pilot_rwy =       f.get_X_pilot_rwy();
 711     // Y_pilot_rwy =       f.get_Y_pilot_rwy();
 712     // H_pilot_rwy =       f.get_H_pilot_rwy();
 713
 714     return( 0 );
 715 }
 716 */
 717
 718
 719