]> git.mxchange.org Git - flightgear.git/blob - src/FDM/LaRCsim/c172_aero.c
Adds a basic FDM model for LaRCsim debugging purposes.
[flightgear.git] / 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