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