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 - http://www.flightgear.org/~curt
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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, 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
do_trims(int kmax,FILE * out,InitialConditions IC)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
find_max_alt(int kmax,InitialConditions IC)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
find_trim_stall(int kmax,FILE * out,InitialConditions IC)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
fgLaRCsimInit(double dt)210 int fgLaRCsimInit(double dt) {
211 ls_toplevel_init(dt);
212
213 return(1);
214 }
215
wave_stats(float * var,float * var_rate,int N,FILE * out)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*LS_PI*LS_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)
main(int argc,char * argv[])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
593