]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/FGTrim.cpp
Adds a basic FDM model for LaRCsim debugging purposes.
[flightgear.git] / src / FDM / JSBSim / FGTrim.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2  
3  Header:       FGTrim.cpp
4  Author:       Tony Peden
5  Date started: 9/8/99
6  
7  --------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) ---------
8  
9  This program is free software; you can redistribute it and/or modify it under
10  the terms of the GNU General Public License as published by the Free Software
11  Foundation; either version 2 of the License, or (at your option) any later
12  version.
13  
14  This program is distributed in the hope that it will be useful, but WITHOUT
15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
17  details.
18  
19  You should have received a copy of the GNU General Public License along with
20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21  Place - Suite 330, Boston, MA  02111-1307, USA.
22  
23  Further information about the GNU General Public License can also be found on
24  the world wide web at http://www.gnu.org.
25  
26  
27  HISTORY
28 --------------------------------------------------------------------------------
29 9/8/99   TP   Created
30  
31  
32 FUNCTIONAL DESCRIPTION
33 --------------------------------------------------------------------------------
34  
35 This class takes the given set of IC's and finds the angle of attack, elevator,
36 and throttle setting required to fly steady level. This is currently for in-air
37 conditions only.  It is implemented using an iterative, one-axis-at-a-time
38 scheme. */
39
40 //  !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!
41
42
43 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
44 INCLUDES
45 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
46
47 #include <stdlib.h>
48
49 #include "FGFDMExec.h"
50 #include "FGAtmosphere.h"
51 #include "FGInitialCondition.h"
52 #include "FGTrim.h"
53 #include "FGAircraft.h"
54 #include "FGMassBalance.h"
55 #include "FGAerodynamics.h"
56 #include "FGColumnVector3.h"
57 #if _MSC_VER
58 #pragma warning (disable : 4786 4788)
59 #endif
60
61 namespace JSBSim {
62
63 static const char *IdSrc = "$Id$";
64 static const char *IdHdr = ID_TRIM;
65
66 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
67
68 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
69
70   N=Nsub=0;
71   max_iterations=60;
72   max_sub_iterations=100;
73   Tolerance=1E-3;
74   A_Tolerance = Tolerance / 10;
75   
76   Debug=0;DebugLevel=0;
77   fdmex=FDMExec;
78   fgic=fdmex->GetIC();
79   total_its=0;
80   trimudot=true;
81   gamma_fallback=true;
82   axis_count=0;
83   mode=tt;
84   xlo=xhi=alo=ahi=0.0;
85   targetNlf=1.0;
86   debug_axis=tAll;
87   SetMode(tt);
88   if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
89 }
90
91 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
92
93 FGTrim::~FGTrim(void) {
94   for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
95     delete TrimAxes[current_axis];
96   }
97   delete[] sub_iterations;
98   delete[] successful;
99   delete[] solution;
100   if (debug_lvl & 2) cout << "Destroyed:    FGTrim" << endl;
101 }
102
103 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
104
105 void FGTrim::TrimStats() {
106   char out[80];
107   int run_sum=0;
108   cout << endl << "  Trim Statistics: " << endl;
109   cout << "    Total Iterations: " << total_its << endl;
110   if(total_its > 0) {
111     cout << "    Sub-iterations:" << endl;
112     for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
113       run_sum+=TrimAxes[current_axis]->GetRunCount();
114       snprintf(out,80,"   %5s: %3.0f average: %5.2f  successful: %3.0f  stability: %5.2f\n",
115                   TrimAxes[current_axis]->GetStateName().c_str(),
116                   sub_iterations[current_axis],
117                   sub_iterations[current_axis]/double(total_its),
118                   successful[current_axis],
119                   TrimAxes[current_axis]->GetAvgStability() );
120       cout << out;
121     }
122     cout << "    Run Count: " << run_sum << endl;
123   }
124 }
125
126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
127
128 void FGTrim::Report(void) {
129   cout << "  Trim Results: " << endl;
130   for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
131     TrimAxes[current_axis]->AxisReport();
132
133 }
134
135 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
136
137 void FGTrim::ClearStates(void) {
138     FGTrimAxis* ta;
139     
140     mode=tCustom;
141     vector<FGTrimAxis*>::iterator iAxes;
142     iAxes = TrimAxes.begin();
143     while (iAxes != TrimAxes.end()) {
144       ta=*iAxes;
145       delete ta;
146       iAxes++;
147     }
148     TrimAxes.clear();
149     //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
150 }
151     
152 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
153
154 bool FGTrim::AddState( State state, Control control ) {
155   FGTrimAxis* ta;
156   bool result=true;
157   
158   mode = tCustom;
159   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
160   while (iAxes != TrimAxes.end()) {
161       ta=*iAxes;
162       if( ta->GetStateType() == state )
163         result=false;
164       iAxes++;
165   }
166   if(result) {
167     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
168     delete[] sub_iterations;
169     delete[] successful;
170     delete[] solution;
171     sub_iterations=new double[TrimAxes.size()];
172     successful=new double[TrimAxes.size()];
173     solution=new bool[TrimAxes.size()];
174   }
175   return result;
176 }  
177
178 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
179
180 bool FGTrim::RemoveState( State state ) {
181   FGTrimAxis* ta;
182   bool result=false;
183   
184   mode = tCustom;
185   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
186   while (iAxes != TrimAxes.end()) {
187       ta=*iAxes;
188       if( ta->GetStateType() == state ) {
189         delete ta;
190         TrimAxes.erase(iAxes);
191         result=true;
192         continue;
193       }
194       iAxes++;
195   }
196   if(result) {
197     delete[] sub_iterations;
198     delete[] successful;
199     delete[] solution;
200     sub_iterations=new double[TrimAxes.size()];
201     successful=new double[TrimAxes.size()];
202     solution=new bool[TrimAxes.size()];
203   }  
204   return result;
205 }  
206
207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
208
209 bool FGTrim::EditState( State state, Control new_control ){       
210   FGTrimAxis* ta;
211   bool result=false;
212   
213   mode = tCustom;
214   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
215   while (iAxes != TrimAxes.end()) {
216       ta=*iAxes;
217       if( ta->GetStateType() == state ) {
218         TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
219         delete ta;
220         TrimAxes.erase(iAxes+1);
221         result=true;
222         break;
223       }
224       iAxes++;
225   }
226   return result;
227 }  
228        
229
230 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
231
232 bool FGTrim::DoTrim(void) {
233   
234   trim_failed=false;
235   int i;
236
237   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
238     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
239   }
240
241   fdmex->GetOutput()->Disable();
242
243   //clear the sub iterations counts & zero out the controls
244   for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
245     //cout << current_axis << "  " << TrimAxes[current_axis]->GetStateName()
246     //<< "  " << TrimAxes[current_axis]->GetControlName()<< endl;
247     if(TrimAxes[current_axis]->GetStateType() == tQdot) {
248       if(mode == tGround) {
249           TrimAxes[current_axis]->initTheta();
250       }  
251     }  
252     xlo=TrimAxes[current_axis]->GetControlMin();
253     xhi=TrimAxes[current_axis]->GetControlMax();
254     TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
255     TrimAxes[current_axis]->Run();
256     //TrimAxes[current_axis]->AxisReport();
257     sub_iterations[current_axis]=0;
258     successful[current_axis]=0;
259     solution[current_axis]=false;
260   }
261   
262   
263   if(mode == tPullup ) {
264     cout << "Setting pitch rate and nlf... " << endl;
265     setupPullup();
266     cout << "pitch rate done ... " << endl;
267     TrimAxes[0]->SetStateTarget(targetNlf);
268     cout << "nlf done" << endl;
269   } else if (mode == tTurn) {
270     setupTurn();
271     //TrimAxes[0]->SetStateTarget(targetNlf);
272   }  
273   
274   do {
275     axis_count=0;
276     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
277       setDebug();
278       updateRates();
279       Nsub=0;
280       if(!solution[current_axis]) {
281         if(checkLimits()) { 
282           solution[current_axis]=true;
283           solve();
284         }  
285       } else if(findInterval()) {
286         solve();
287       } else {
288         solution[current_axis]=false;
289       }  
290       sub_iterations[current_axis]+=Nsub;
291     } 
292     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
293       //these checks need to be done after all the axes have run
294       if(Debug > 0) TrimAxes[current_axis]->AxisReport();
295       if(TrimAxes[current_axis]->InTolerance()) {
296         axis_count++;
297         successful[current_axis]++;
298       } 
299     }
300     
301
302     if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
303       //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
304       //At this point we can check the input limits of the failed axis
305       //and declare the trim failed if there is no sign change. If there
306       //is, keep going until success or max iteration count
307
308       //Oh, well: two out of three ain't bad
309       for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
310         //these checks need to be done after all the axes have run
311         if(!TrimAxes[current_axis]->InTolerance()) {
312           if(!checkLimits()) {
313             // special case this for now -- if other cases arise proper
314             // support can be added to FGTrimAxis
315             if( (gamma_fallback) &&
316                 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
317                 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
318               cout << "  Can't trim udot with throttle, trying flight"
319               << " path angle. (" << N << ")" << endl;
320               if(TrimAxes[current_axis]->GetState() > 0)
321                 TrimAxes[current_axis]->SetControlToMin();
322               else
323                 TrimAxes[current_axis]->SetControlToMax();
324               TrimAxes[current_axis]->Run();
325               delete TrimAxes[current_axis];
326               TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
327                                                     tGamma );
328             } else {
329               cout << "  Sorry, " << TrimAxes[current_axis]->GetStateName()
330               << " doesn't appear to be trimmable" << endl;
331               //total_its=k;
332               trim_failed=true; //force the trim to fail
333             } //gamma_fallback
334           }
335         } //solution check
336       } //for loop
337     } //all-but-one check
338     N++;
339     if(N > max_iterations)
340       trim_failed=true;
341   } while((axis_count < TrimAxes.size()) && (!trim_failed));
342   if((!trim_failed) && (axis_count >= TrimAxes.size())) {
343     total_its=N;
344     cout << endl << "  Trim successful" << endl;
345   } else {
346     total_its=N;
347     cout << endl << "  Trim failed" << endl;
348   }
349   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
350     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
351   }
352   fdmex->GetOutput()->Enable();
353   return !trim_failed;
354 }
355
356 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
357
358 bool FGTrim::solve(void) {
359
360   double x1,x2,x3,f1,f2,f3,d,d0;
361   const double relax =0.9;
362   double eps=TrimAxes[current_axis]->GetSolverEps();
363
364   x1=x2=x3=0;
365   d=1;
366   bool success=false;
367   //initializations
368   if( solutionDomain != 0) {
369    /* if(ahi > alo) { */
370       x1=xlo;f1=alo;
371       x3=xhi;f3=ahi;
372    /* } else {
373       x1=xhi;f1=ahi;
374       x3=xlo;f3=alo;
375     }   */
376     d0=fabs(x3-x1);
377     //iterations
378     //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
379     while ( (TrimAxes[current_axis]->InTolerance() == false )
380              && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
381       Nsub++;
382       d=(x3-x1)/d0;
383       x2=x1-d*d0*f1/(f3-f1);
384       TrimAxes[current_axis]->SetControl(x2);
385       TrimAxes[current_axis]->Run();
386       f2=TrimAxes[current_axis]->GetState();
387       if(Debug > 1) {
388         cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
389         << ", " << x2 << ", " << x3 << endl;
390         cout << "                             " << f1 << ", " << f2 << ", " << f3 << endl;
391       }
392       if(f1*f2 <= 0.0) {
393         x3=x2;
394         f3=f2;
395         f1=relax*f1;
396         //cout << "Solution is between x1 and x2" << endl;
397       }
398       else if(f2*f3 <= 0.0) {
399         x1=x2;
400         f1=f2;
401         f3=relax*f3;
402         //cout << "Solution is between x2 and x3" << endl;
403
404       }
405       //cout << i << endl;
406
407       
408     }//end while
409     if(Nsub < max_sub_iterations) success=true;
410   }  
411   return success;
412 }
413
414 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
415 /*
416  produces an interval (xlo..xhi) on one side or the other of the current 
417  control value in which a solution exists.  This domain is, hopefully, 
418  smaller than xmin..0 or 0..xmax and the solver will require fewer iterations 
419  to find the solution. This is, hopefully, more efficient than having the 
420  solver start from scratch every time. Maybe it isn't though...
421  This tries to take advantage of the idea that the changes from iteration to
422  iteration will be small after the first one or two top-level iterations.
423
424  assumes that changing the control will a produce significant change in the
425  accel i.e. checkLimits() has already been called.
426
427  if a solution is found above the current control, the function returns true 
428  and xlo is set to the current control, xhi to the interval max it found, and 
429  solutionDomain is set to 1.
430  if the solution lies below the current control, then the function returns 
431  true and xlo is set to the interval min it found and xmax to the current 
432  control. if no solution is found, then the function returns false.
433  
434  
435  in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
436  no assumptions about the state of the sim after this function has run 
437  can be made.
438 */
439 bool FGTrim::findInterval(void) {
440   bool found=false;
441   double step;
442   double current_control=TrimAxes[current_axis]->GetControl();
443   double current_accel=TrimAxes[current_axis]->GetState();;
444   double xmin=TrimAxes[current_axis]->GetControlMin();
445   double xmax=TrimAxes[current_axis]->GetControlMax();
446   double lastxlo,lastxhi,lastalo,lastahi;
447   
448   step=0.025*fabs(xmax);
449   xlo=xhi=current_control;
450   alo=ahi=current_accel;
451   lastxlo=xlo;lastxhi=xhi;
452   lastalo=alo;lastahi=ahi;
453   do {
454     
455     Nsub++;
456     step*=2;
457     xlo-=step;
458     if(xlo < xmin) xlo=xmin;
459     xhi+=step;
460     if(xhi > xmax) xhi=xmax;
461     TrimAxes[current_axis]->SetControl(xlo);
462     TrimAxes[current_axis]->Run();
463     alo=TrimAxes[current_axis]->GetState();
464     TrimAxes[current_axis]->SetControl(xhi);
465     TrimAxes[current_axis]->Run();
466     ahi=TrimAxes[current_axis]->GetState();
467     if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
468     if(alo*ahi <=0) {  //found interval with root
469       found=true;
470       if(alo*current_accel <= 0) { //narrow interval down a bit
471         solutionDomain=-1;
472         xhi=lastxlo;
473         ahi=lastalo;
474         //xhi=current_control;
475         //ahi=current_accel;
476       } else {
477         solutionDomain=1;
478         xlo=lastxhi;
479         alo=lastahi;
480         //xlo=current_control;
481         //alo=current_accel;
482       }     
483     }
484     lastxlo=xlo;lastxhi=xhi;
485     lastalo=alo;lastahi=ahi;
486     if( !found && xlo==xmin && xhi==xmax ) continue;
487     if(Debug > 1)
488       cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
489                            << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
490   } while(!found && (Nsub <= max_sub_iterations) );
491   return found;
492 }
493
494 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
495 //checks to see which side of the current control value the solution is on
496 //and sets solutionDomain accordingly:
497 //  1 if solution is between the current and max
498 // -1 if solution is between the min and current
499 //  0 if there is no solution
500 // 
501 //if changing the control produces no significant change in the accel then
502 //solutionDomain is set to zero and the function returns false
503 //if a solution is found, then xlo and xhi are set so that they bracket
504 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
505 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
506 //xhi=xmax and ahi=accel(xmax) 
507 //in all cases the sim is left such that the control=xmax and accel=ahi
508
509 bool FGTrim::checkLimits(void) {
510   bool solutionExists;
511   double current_control=TrimAxes[current_axis]->GetControl();
512   double current_accel=TrimAxes[current_axis]->GetState();
513   xlo=TrimAxes[current_axis]->GetControlMin();
514   xhi=TrimAxes[current_axis]->GetControlMax();
515
516   TrimAxes[current_axis]->SetControl(xlo);
517   TrimAxes[current_axis]->Run();
518   alo=TrimAxes[current_axis]->GetState();
519   TrimAxes[current_axis]->SetControl(xhi);
520   TrimAxes[current_axis]->Run();
521   ahi=TrimAxes[current_axis]->GetState();
522   if(Debug > 1)
523     cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
524                                               << alo << ", " << ahi << endl;
525   solutionDomain=0;
526   solutionExists=false;
527   if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
528     if(alo*current_accel <= 0) {
529       solutionExists=true;
530       solutionDomain=-1;
531       xhi=current_control;
532       ahi=current_accel;
533     } else if(current_accel*ahi < 0){
534       solutionExists=true;
535       solutionDomain=1;
536       xlo=current_control;
537       alo=current_accel;  
538     }
539   } 
540   TrimAxes[current_axis]->SetControl(current_control);
541   TrimAxes[current_axis]->Run();
542   return solutionExists;
543 }
544
545 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
546
547 void FGTrim::setupPullup() {
548   float g,q,cgamma;
549   FGColumnVector3 vPQR;
550   g=fdmex->GetInertial()->gravity();
551   cgamma=cos(fgic->GetFlightPathAngleRadIC());
552   cout << "setPitchRateInPullup():  " << g << ", " << cgamma << ", "
553        << fgic->GetVtrueFpsIC() << endl;
554   q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
555   cout << targetNlf << ", " << q << endl;
556   fdmex->GetRotation()->SetPQR(0,q,0);
557   cout << "setPitchRateInPullup() complete" << endl;
558   
559 }  
560
561 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
562
563 void FGTrim::setupTurn(void){
564   double g,phi;
565   phi = fgic->GetRollAngleRadIC();
566   if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
567     targetNlf = 1 / cos(phi);
568     g = fdmex->GetInertial()->gravity(); 
569     psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
570     cout << targetNlf << ", " << psidot << endl;
571   }
572    
573 }  
574
575 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
576
577 void FGTrim::updateRates(void){
578   if( mode == tTurn ) {
579     double phi = fgic->GetRollAngleRadIC();
580     double g = fdmex->GetInertial()->gravity(); 
581     double p,q,r,theta;
582     if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
583       theta=fgic->GetPitchAngleRadIC();
584       phi=fgic->GetRollAngleRadIC();
585       psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
586       p=-psidot*sin(theta);
587       q=psidot*cos(theta)*sin(phi);
588       r=psidot*cos(theta)*cos(phi);
589     } else {
590       p=q=r=0;
591     }      
592     fdmex->GetRotation()->SetPQR(p,q,r);
593   } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
594       float g,q,cgamma;
595       FGColumnVector3 vPQR;
596       g=fdmex->GetInertial()->gravity();
597       cgamma=cos(fgic->GetFlightPathAngleRadIC());
598       q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
599       fdmex->GetRotation()->SetPQR(0,q,0);
600   }  
601 }  
602
603 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
604
605 void FGTrim::setDebug(void) {
606   if(debug_axis == tAll ||
607       TrimAxes[current_axis]->GetStateType() == debug_axis ) {
608     Debug=DebugLevel; 
609     return;
610   } else {
611     Debug=0;
612     return;
613   }
614 }      
615
616 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
617
618 void FGTrim::SetMode(TrimMode tt) {
619     ClearStates();
620     mode=tt;
621     switch(tt) {
622       case tFull:
623         cout << "  Full Trim" << endl;
624         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
625         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
626         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
627         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
628         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
629         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
630         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
631         break;
632       case tLongitudinal:
633         cout << "  Longitudinal Trim" << endl;
634         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
635         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
636         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
637         break;
638       case tGround:
639         cout << "  Ground Trim" << endl;
640         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
641         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
642         //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
643         break;
644       case tPullup:
645         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
646         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
647         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
648         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
649         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
650         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
651         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
652         break;
653       case tTurn:
654         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
655         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
656         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
657         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
658         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
659         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
660         break;
661       case tCustom:
662       case tNone:
663         break;
664     }
665     //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
666     sub_iterations=new double[TrimAxes.size()];
667     successful=new double[TrimAxes.size()];
668     solution=new bool[TrimAxes.size()];
669     current_axis=0;
670 }    
671 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
672 }