src/FDM/LaRCsim/c172_aero.c

   1 /***************************************************************************
   2
   3   TITLE:        c172_aero
   4
   5 ----------------------------------------------------------------------------
   6
   7   FUNCTION:     aerodynamics model based on constant stability derivatives
   8
   9 ----------------------------------------------------------------------------
  10
  11   MODULE STATUS:        developmental
  12
  13 ----------------------------------------------------------------------------
  14
  15   GENEALOGY:    Based on data from:
  16                                 Part 1 of Roskam's S&C text
  17                                 The FAA type certificate data sheet for the 172
  18                                 Various sources on the net
  19                                 John D. Anderson's Intro to Flight text (NACA 2412 data)
  20                                 UIUC's airfoil data web site
  21
  22 ----------------------------------------------------------------------------
  23
  24   DESIGNED BY:  Tony Peden
  25
  26   CODED BY:             Tony Peden
  27
  28   MAINTAINED BY:        Tony Peden
  29
  30 ----------------------------------------------------------------------------
  31
  32   MODIFICATION HISTORY:
  33
  34   DATE          PURPOSE                                                                                         BY
  35   6/10/99   Initial test release
  36
  37
  38 ----------------------------------------------------------------------------
  39
  40   REFERENCES:
  41
  42   Aero Coeffs:
  43         CL                      lift
  44         Cd                      drag
  45         Cm                      pitching moment
  46         Cy                      sideforce
  47         Cn                      yawing moment
  48         Croll,Cl        rolling moment (yeah, I know.  Shoot me.)
  49
  50   Subscripts
  51         o               constant i.e. not a function of alpha or beta
  52         a               alpha
  53         adot    d(alpha)/dt
  54         q       pitch rate
  55         qdot    d(q)/dt
  56         beta    sideslip angle
  57         p               roll rate
  58         r               yaw rate
  59         da      aileron deflection
  60         de      elevator deflection
  61         dr      rudder deflection
  62
  63         s               stability axes
  64
  65
  66
  67 ----------------------------------------------------------------------------
  68
  69   CALLED BY:
  70
  71 ----------------------------------------------------------------------------
  72
  73   CALLS TO:
  74
  75 ----------------------------------------------------------------------------
  76
  77   INPUTS:
  78
  79 ----------------------------------------------------------------------------
  80
  81   OUTPUTS:
  82
  83 --------------------------------------------------------------------------*/
  84
  85
  86
  87 #include "ls_generic.h"
  88 #include "ls_cockpit.h"
  89 #include "ls_constants.h"
  90 #include "ls_types.h"
  91 #include "c172_aero.h"
  92
  93 #include <math.h>
  94 #include <stdio.h>
  95
  96
  97 #define NCL 9
  98 #define Ndf 4
  99 #define DYN_ON_SPEED 33 /*20 knots*/
 100
 101
 102 #ifdef USENZ
 103         #define NZ generic_.n_cg_body_v[2]
 104 #else
 105         #define NZ 1
 106 #endif
 107
 108
 109 extern COCKPIT cockpit_;
 110
 111
 112    SCALAR CLadot;
 113    SCALAR CLq;
 114    SCALAR CLde;
 115    SCALAR CLob;
 116
 117
 118    SCALAR Cdob;
 119    SCALAR Cda;  /*Not used*/
 120    SCALAR Cdde;
 121
 122    SCALAR Cma;
 123    SCALAR Cmadot;
 124    SCALAR Cmq;
 125    SCALAR Cmob;
 126    SCALAR Cmde;
 127
 128    SCALAR Clbeta;
 129    SCALAR Clp;
 130    SCALAR Clr;
 131    SCALAR Clda;
 132    SCALAR Cldr;
 133
 134    SCALAR Cnbeta;
 135    SCALAR Cnp;
 136    SCALAR Cnr;
 137    SCALAR Cnda;
 138    SCALAR Cndr;
 139
 140    SCALAR Cybeta;
 141    SCALAR Cyp;
 142    SCALAR Cyr;
 143    SCALAR Cyda;
 144    SCALAR Cydr;
 145
 146   /*nondimensionalization quantities*/
 147   /*units here are ft and lbs */
 148    SCALAR cbar; /*mean aero chord ft*/
 149    SCALAR b; /*wing span ft */
 150    SCALAR Sw; /*wing planform surface area ft^2*/
 151    SCALAR rPiARe; /*reciprocal of Pi*AR*e*/
 152    SCALAR lbare;  /*elevator moment arm  MAC*/
 153
 154    SCALAR Weight; /*lbs*/
 155    SCALAR MaxTakeoffWeight,EmptyWeight;
 156    SCALAR Cg;     /*%MAC*/
 157    SCALAR Zcg;    /*%MAC*/
 158
 159
 160   SCALAR CLwbh,CL,cm,cd,cn,cy,croll,cbar_2V,b_2V,qS,qScbar,qSb;
 161   SCALAR CLo,Cdo,Cmo;
 162
 163   SCALAR F_X_wind,F_Y_wind,F_Z_wind;
 164
 165   SCALAR long_trim;
 166
 167
 168   SCALAR elevator, aileron, rudder;
 169
 170
 171   SCALAR Flap_Position;
 172
 173   int Flaps_In_Transit;
 174
 175 static SCALAR interp(SCALAR *y_table, SCALAR *x_table, int Ntable, SCALAR x)
 176 {
 177         SCALAR slope;
 178         int i=1;
 179         float y;
 180
 181
 182         /* if x is outside the table, return value at x[0] or x[Ntable-1]*/
 183         if(x <= x_table[0])
 184         {
 185                  y=y_table[0];
 186                  /* printf("x smaller than x_table[0]: %g %g\n",x,x_table[0]); */
 187         }
 188         else if(x >= x_table[Ntable-1])
 189         {
 190                 slope=(y_table[Ntable-1]-y_table[Ntable-2])/(x_table[Ntable-1]-x_table[Ntable-2]);
 191             y=slope*(x-x_table[Ntable-1]) +y_table[Ntable-1];
 192
 193 /*               printf("x larger than x_table[N]: %g %g %d\n",x,x_table[NCL-1],Ntable-1);
 194  */     }
 195         else /*x is within the table, interpolate linearly to find y value*/
 196         {
 197
 198             while(x_table[i] <= x) {i++;}
 199             slope=(y_table[i]-y_table[i-1])/(x_table[i]-x_table[i-1]);
 200                 /* printf("x: %g, i: %d, cl[i]: %g, cl[i-1]: %g, slope: %g\n",x,i,y_table[i],y_table[i-1],slope); */
 201             y=slope*(x-x_table[i-1]) +y_table[i-1];
 202         }
 203         return y;
 204 }
 205
 206
 207 void c172_aero( SCALAR dt, int Initialize ) {
 208
 209
 210   // static int init = 0;
 211   static int fi=0;
 212   static SCALAR lastFlapHandle=0;
 213   static SCALAR Ai;
 214
 215   static SCALAR trim_inc = 0.0002;
 216
 217   static SCALAR alpha_ind[NCL]={-0.087,0,0.14,0.21,0.24,0.26,0.28,0.31,0.35};
 218   static SCALAR CLtable[NCL]={-0.22,0.25,1.02,1.252,1.354,1.44,1.466,1.298,0.97};
 219
 220   static SCALAR flap_ind[Ndf]={0,10,20,30};
 221   static SCALAR flap_times[Ndf]={0,4,2,2};
 222   static SCALAR dCLf[Ndf]={0,0.20,0.30,0.35};
 223   static SCALAR dCdf[Ndf]={0,0.0021,0.0085,0.0191};
 224   static SCALAR dCmf[Ndf]={0,-0.0654,-0.0981,-0.114};
 225
 226   static SCALAR flap_transit_rate=2.5;
 227
 228
 229
 230
 231
 232    /* printf("Initialize= %d\n",Initialize); */
 233 /*         printf("Initializing aero model...Initialize= %d\n", Initialize);
 234  */
 235         /*nondimensionalization quantities*/
 236            /*units here are ft and lbs */
 237            cbar=4.9; /*mean aero chord ft*/
 238            b=35.8; /*wing span ft */
 239            Sw=174; /*wing planform surface area ft^2*/
 240            rPiARe=0.054; /*reciprocal of Pi*AR*e*/
 241            lbare=3.682;  /*elevator moment arm / MAC*/
 242
 243            CLadot=1.7;
 244            CLq=3.9;
 245
 246            CLob=0;
 247
 248        Ai=1.24;
 249            Cdob=0.036;
 250            Cda=0.13;  /*Not used*/
 251            Cdde=0.06;
 252
 253            Cma=-1.8;
 254            Cmadot=-5.2;
 255            Cmq=-12.4;
 256            Cmob=-0.02;
 257            Cmde=-1.28;
 258
 259            CLde=-Cmde / lbare; /* kinda backwards, huh? */
 260
 261            Clbeta=-0.089;
 262            Clp=-0.47;
 263            Clr=0.096;
 264            Clda=-0.09;
 265            Cldr=0.0147;
 266
 267            Cnbeta=0.065;
 268            Cnp=-0.03;
 269            Cnr=-0.099;
 270            Cnda=-0.0053;
 271            Cndr=-0.0657;
 272
 273            Cybeta=-0.31;
 274            Cyp=-0.037;
 275            Cyr=0.21;
 276            Cyda=0.0;
 277            Cydr=0.187;
 278
 279
 280
 281            MaxTakeoffWeight=2550;
 282            EmptyWeight=1500;
 283
 284            Zcg=0.51;
 285
 286   /*
 287   LaRCsim uses:
 288     Cm > 0 => ANU
 289         Cl > 0 => Right wing down
 290         Cn > 0 => ANL
 291   so:
 292     elevator > 0 => AND -- aircraft nose down
 293         aileron > 0  => right wing up
 294         rudder > 0   => ANL
 295   */
 296
 297   /*do weight & balance here since there is no better place*/
 298   Weight=Mass / INVG;
 299
 300   if(Weight > 2550)
 301   {  Weight=2550; }
 302   else if(Weight < 1500)
 303   {  Weight=1500; }
 304
 305
 306   if(Dx_cg > 0.43)
 307   {  Dx_cg = 0.43; }
 308   else if(Dx_cg < -0.6)
 309   {  Dx_cg = -0.6; }
 310
 311   Cg=0.25 - Dx_cg/cbar;
 312
 313   Dz_cg=Zcg*cbar;
 314   Dy_cg=0;
 315
 316
 317   if(Flap_handle < flap_ind[0])
 318   {
 319         fi=0;
 320         Flap_handle=flap_ind[0];
 321         lastFlapHandle=Flap_handle;
 322         Flap_Position=flap_ind[0];
 323   }
 324   else if(Flap_handle > flap_ind[Ndf-1])
 325   {
 326          fi=Ndf-1;
 327          Flap_handle=flap_ind[fi];
 328          lastFlapHandle=Flap_handle;
 329          Flap_Position=flap_ind[fi];
 330   }
 331   else
 332   {
 333
 334          if(dt <= 0)
 335             Flap_Position=Flap_handle;
 336          else
 337          {
 338                 if(Flap_handle != lastFlapHandle)
 339                 {
 340                    Flaps_In_Transit=1;
 341                 }
 342                 if(Flaps_In_Transit)
 343                 {
 344                    fi=0;
 345                while(flap_ind[fi] < Flap_handle) { fi++; }
 346                    if(Flap_Position < Flap_handle)
 347                    {
 348                if(flap_times[fi] > 0)
 349                                 flap_transit_rate=(flap_ind[fi] - flap_ind[fi-1])/flap_times[fi];
 350                            else
 351                                 flap_transit_rate=(flap_ind[fi] - flap_ind[fi-1])/5;
 352                    }
 353                    else
 354                    {
 355                         if(flap_times[fi+1] > 0)
 356                                    flap_transit_rate=(flap_ind[fi] - flap_ind[fi+1])/flap_times[fi+1];
 357                                 else
 358                                flap_transit_rate=(flap_ind[fi] - flap_ind[fi+1])/5;
 359                    }
 360                    if(fabs(Flap_Position - Flap_handle) > dt*flap_transit_rate)
 361                            Flap_Position+=flap_transit_rate*dt;
 362                    else
 363                    {
 364                            Flaps_In_Transit=0;
 365                            Flap_Position=Flap_handle;
 366                    }
 367         }
 368          }
 369          lastFlapHandle=Flap_handle;
 370   }
 371
 372   if(Aft_trim) long_trim = long_trim - trim_inc;
 373   if(Fwd_trim) long_trim = long_trim + trim_inc;
 374
 375 /*   printf("Long_control: %7.4f, long_trim: %7.4f,DEG_TO_RAD: %7.4f, RAD_TO_DEG: %7.4f\n",Long_control,long_trim,DEG_TO_RAD,RAD_TO_DEG);
 376  */  /*scale pct control to degrees deflection*/
 377   if ((Long_control+Long_trim) <= 0)
 378         elevator=(Long_control+Long_trim)*28*DEG_TO_RAD;
 379   else
 380         elevator=(Long_control+Long_trim)*23*DEG_TO_RAD;
 381
 382   aileron  = -1*Lat_control*17.5*DEG_TO_RAD;
 383   rudder   = -1*Rudder_pedal*16*DEG_TO_RAD;
 384   /*
 385     The aileron travel limits are 20 deg. TEU and 15 deg TED
 386     but since we don't distinguish between left and right we'll
 387         use the average here (17.5 deg)
 388   */
 389
 390
 391   /*calculate rate derivative nondimensionalization (is that a word?) factors */
 392   /*hack to avoid divide by zero*/
 393   /*the dynamic terms are negligible at low ground speeds anyway*/
 394
 395 /*   printf("Vinf: %g, Span: %g\n",V_rel_wind,b);
 396  */
 397   if(V_rel_wind > DYN_ON_SPEED)
 398   {
 399         cbar_2V=cbar/(2*V_rel_wind);
 400         b_2V=b/(2*V_rel_wind);
 401   }
 402   else
 403   {
 404         cbar_2V=0;
 405         b_2V=0;
 406   }
 407
 408
 409   /*calcuate the qS nondimensionalization factors*/
 410
 411   qS=Dynamic_pressure*Sw;
 412   qScbar=qS*cbar;
 413   qSb=qS*b;
 414
 415
 416 /*   printf("aero: Wb: %7.4f, Ub: %7.4f, Alpha: %7.4f, elev: %7.4f, ail: %7.4f, rud: %7.4f, long_trim: %7.4f\n",W_body,U_body,Alpha*RAD_TO_DEG,elevator*RAD_TO_DEG,aileron*RAD_TO_DEG,rudder*RAD_TO_DEG,long_trim*RAD_TO_DEG);
 417   printf("aero: Theta: %7.4f, Gamma: %7.4f, Beta: %7.4f, Phi: %7.4f, Psi: %7.4f\n",Theta*RAD_TO_DEG,Gamma_vert_rad*RAD_TO_DEG,Beta*RAD_TO_DEG,Phi*RAD_TO_DEG,Psi*RAD_TO_DEG);
 418  */
 419
 420
 421
 422   /* sum coefficients */
 423   CLwbh = interp(CLtable,alpha_ind,NCL,Std_Alpha);
 424 /*   printf("CLwbh: %g\n",CLwbh);
 425  */
 426   CLo = CLob + interp(dCLf,flap_ind,Ndf,Flap_Position);
 427   Cdo = Cdob + interp(dCdf,flap_ind,Ndf,Flap_Position);
 428   Cmo = Cmob + interp(dCmf,flap_ind,Ndf,Flap_Position);
 429
 430   /* printf("FP: %g\n",Flap_Position);
 431   printf("CLo: %g\n",CLo);
 432   printf("Cdo: %g\n",Cdo);
 433   printf("Cmo: %g\n",Cmo);        */
 434
 435
 436
 437
 438
 439   CL = CLo + CLwbh + (CLadot*Std_Alpha_dot + CLq*Theta_dot)*cbar_2V + CLde*elevator;
 440   cd = Cdo + rPiARe*Ai*Ai*CL*CL + Cdde*elevator;
 441   cy = Cybeta*Std_Beta + (Cyp*P_body + Cyr*R_body)*b_2V + Cyda*aileron + Cydr*rudder;
 442
 443   cm = Cmo + Cma*Std_Alpha + (Cmq*Q_body + Cmadot*Std_Alpha_dot)*cbar_2V + Cmde*(elevator);
 444   cn = Cnbeta*Std_Beta + (Cnp*P_body + Cnr*R_body)*b_2V + Cnda*aileron + Cndr*rudder;
 445   croll=Clbeta*Std_Beta + (Clp*P_body + Clr*R_body)*b_2V + Clda*aileron + Cldr*rudder;
 446
 447 /*   printf("aero: CL: %7.4f, Cd: %7.4f, Cm: %7.4f, Cy: %7.4f, Cn: %7.4f, Cl: %7.4f\n",CL,cd,cm,cy,cn,croll);
 448  */
 449
 450   /*calculate wind axes forces*/
 451   F_X_wind=-1*cd*qS;
 452   F_Y_wind=cy*qS;
 453   F_Z_wind=-1*CL*qS;
 454
 455 /*   printf("V_rel_wind: %7.4f, Fxwind: %7.4f Fywind: %7.4f Fzwind: %7.4f\n",V_rel_wind,F_X_wind,F_Y_wind,F_Z_wind);
 456  */
 457
 458   /*calculate moments and body axis forces */
 459
 460
 461
 462   /* requires ugly wind-axes to body-axes transform */
 463   F_X_aero = F_X_wind*Cos_alpha*Cos_beta - F_Y_wind*Cos_alpha*Sin_beta - F_Z_wind*Sin_alpha;
 464   F_Y_aero = F_X_wind*Sin_beta + F_Y_wind*Cos_beta;
 465   F_Z_aero = F_X_wind*Sin_alpha*Cos_beta - F_Y_wind*Sin_alpha*Sin_beta + F_Z_wind*Cos_alpha;
 466
 467   /*no axes transform here */
 468   M_l_aero = croll*qSb;
 469   M_m_aero = cm*qScbar;
 470   M_n_aero = cn*qSb;
 471
 472 /*   printf("I_yy: %7.4f, qScbar: %7.4f, qbar: %7.4f, Sw: %7.4f, cbar: %7.4f, 0.5*rho*V^2: %7.4f\n",I_yy,qScbar,Dynamic_pressure,Sw,cbar,0.5*0.0023081*V_rel_wind*V_rel_wind);
 473
 474   printf("Fxaero: %7.4f Fyaero: %7.4f Fzaero: %7.4f Weight: %7.4f\n",F_X_aero,F_Y_aero,F_Z_aero,Weight);
 475
 476   printf("Maero: %7.4f Naero: %7.4f Raero: %7.4f\n",M_m_aero,M_n_aero,M_l_aero);
 477   */
 478
 479 }
 480
 481