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