]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/FGTrim.cpp
Connect the FlightGear /environment/turbulence-norm property to the
[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     xlo=TrimAxes[current_axis]->GetControlMin();
252     xhi=TrimAxes[current_axis]->GetControlMax();
253     TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
254     TrimAxes[current_axis]->Run();
255     //TrimAxes[current_axis]->AxisReport();
256     sub_iterations[current_axis]=0;
257     successful[current_axis]=0;
258     solution[current_axis]=false;
259   }
260   
261   
262   if(mode == tPullup ) {
263     cout << "Setting pitch rate and nlf... " << endl;
264     setupPullup();
265     cout << "pitch rate done ... " << endl;
266     TrimAxes[0]->SetStateTarget(targetNlf);
267     cout << "nlf done" << endl;
268   } else if (mode == tTurn) {
269     setupTurn();
270     //TrimAxes[0]->SetStateTarget(targetNlf);
271   }  
272   
273   do {
274     axis_count=0;
275     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
276       setDebug();
277       updateRates();
278       Nsub=0;
279       if(!solution[current_axis]) {
280         if(checkLimits()) { 
281           solution[current_axis]=true;
282           solve();
283         }  
284       } else if(findInterval()) {
285         solve();
286       } else {
287         solution[current_axis]=false;
288       }  
289       sub_iterations[current_axis]+=Nsub;
290     } 
291     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
292       //these checks need to be done after all the axes have run
293       if(Debug > 0) TrimAxes[current_axis]->AxisReport();
294       if(TrimAxes[current_axis]->InTolerance()) {
295         axis_count++;
296         successful[current_axis]++;
297       } 
298     }
299     
300
301     if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
302       //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
303       //At this point we can check the input limits of the failed axis
304       //and declare the trim failed if there is no sign change. If there
305       //is, keep going until success or max iteration count
306
307       //Oh, well: two out of three ain't bad
308       for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
309         //these checks need to be done after all the axes have run
310         if(!TrimAxes[current_axis]->InTolerance()) {
311           if(!checkLimits()) {
312             // special case this for now -- if other cases arise proper
313             // support can be added to FGTrimAxis
314             if( (gamma_fallback) &&
315                 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
316                 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
317               cout << "  Can't trim udot with throttle, trying flight"
318               << " path angle. (" << N << ")" << endl;
319               if(TrimAxes[current_axis]->GetState() > 0)
320                 TrimAxes[current_axis]->SetControlToMin();
321               else
322                 TrimAxes[current_axis]->SetControlToMax();
323               TrimAxes[current_axis]->Run();
324               delete TrimAxes[current_axis];
325               TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
326                                                     tGamma );
327             } else {
328               cout << "  Sorry, " << TrimAxes[current_axis]->GetStateName()
329               << " doesn't appear to be trimmable" << endl;
330               //total_its=k;
331               trim_failed=true; //force the trim to fail
332             } //gamma_fallback
333           }
334         } //solution check
335       } //for loop
336     } //all-but-one check
337     N++;
338     if(N > max_iterations)
339       trim_failed=true;
340   } while((axis_count < TrimAxes.size()) && (!trim_failed));
341   if((!trim_failed) && (axis_count >= TrimAxes.size())) {
342     total_its=N;
343     cout << endl << "  Trim successful" << endl;
344   } else {
345     total_its=N;
346     cout << endl << "  Trim failed" << endl;
347   }
348   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
349     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
350   }
351   fdmex->GetOutput()->Enable();
352   return !trim_failed;
353 }
354
355 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
356
357 bool FGTrim::solve(void) {
358
359   double x1,x2,x3,f1,f2,f3,d,d0;
360   const double relax =0.9;
361   double eps=TrimAxes[current_axis]->GetSolverEps();
362
363   x1=x2=x3=0;
364   d=1;
365   bool success=false;
366   //initializations
367   if( solutionDomain != 0) {
368    /* if(ahi > alo) { */
369       x1=xlo;f1=alo;
370       x3=xhi;f3=ahi;
371    /* } else {
372       x1=xhi;f1=ahi;
373       x3=xlo;f3=alo;
374     }   */
375     d0=fabs(x3-x1);
376     //iterations
377     //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
378     while ( (TrimAxes[current_axis]->InTolerance() == false )
379              && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
380       Nsub++;
381       d=(x3-x1)/d0;
382       x2=x1-d*d0*f1/(f3-f1);
383       TrimAxes[current_axis]->SetControl(x2);
384       TrimAxes[current_axis]->Run();
385       f2=TrimAxes[current_axis]->GetState();
386       if(Debug > 1) {
387         cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
388         << ", " << x2 << ", " << x3 << endl;
389         cout << "                             " << f1 << ", " << f2 << ", " << f3 << endl;
390       }
391       if(f1*f2 <= 0.0) {
392         x3=x2;
393         f3=f2;
394         f1=relax*f1;
395         //cout << "Solution is between x1 and x2" << endl;
396       }
397       else if(f2*f3 <= 0.0) {
398         x1=x2;
399         f1=f2;
400         f3=relax*f3;
401         //cout << "Solution is between x2 and x3" << endl;
402
403       }
404       //cout << i << endl;
405
406       
407     }//end while
408     if(Nsub < max_sub_iterations) success=true;
409   }  
410   return success;
411 }
412
413 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
414 /*
415  produces an interval (xlo..xhi) on one side or the other of the current 
416  control value in which a solution exists.  This domain is, hopefully, 
417  smaller than xmin..0 or 0..xmax and the solver will require fewer iterations 
418  to find the solution. This is, hopefully, more efficient than having the 
419  solver start from scratch every time. Maybe it isn't though...
420  This tries to take advantage of the idea that the changes from iteration to
421  iteration will be small after the first one or two top-level iterations.
422
423  assumes that changing the control will a produce significant change in the
424  accel i.e. checkLimits() has already been called.
425
426  if a solution is found above the current control, the function returns true 
427  and xlo is set to the current control, xhi to the interval max it found, and 
428  solutionDomain is set to 1.
429  if the solution lies below the current control, then the function returns 
430  true and xlo is set to the interval min it found and xmax to the current 
431  control. if no solution is found, then the function returns false.
432  
433  
434  in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
435  no assumptions about the state of the sim after this function has run 
436  can be made.
437 */
438 bool FGTrim::findInterval(void) {
439   bool found=false;
440   double step;
441   double current_control=TrimAxes[current_axis]->GetControl();
442   double current_accel=TrimAxes[current_axis]->GetState();;
443   double xmin=TrimAxes[current_axis]->GetControlMin();
444   double xmax=TrimAxes[current_axis]->GetControlMax();
445   double lastxlo,lastxhi,lastalo,lastahi;
446   
447   step=0.025*fabs(xmax);
448   xlo=xhi=current_control;
449   alo=ahi=current_accel;
450   lastxlo=xlo;lastxhi=xhi;
451   lastalo=alo;lastahi=ahi;
452   do {
453     
454     Nsub++;
455     step*=2;
456     xlo-=step;
457     if(xlo < xmin) xlo=xmin;
458     xhi+=step;
459     if(xhi > xmax) xhi=xmax;
460     TrimAxes[current_axis]->SetControl(xlo);
461     TrimAxes[current_axis]->Run();
462     alo=TrimAxes[current_axis]->GetState();
463     TrimAxes[current_axis]->SetControl(xhi);
464     TrimAxes[current_axis]->Run();
465     ahi=TrimAxes[current_axis]->GetState();
466     if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
467     if(alo*ahi <=0) {  //found interval with root
468       found=true;
469       if(alo*current_accel <= 0) { //narrow interval down a bit
470         solutionDomain=-1;
471         xhi=lastxlo;
472         ahi=lastalo;
473         //xhi=current_control;
474         //ahi=current_accel;
475       } else {
476         solutionDomain=1;
477         xlo=lastxhi;
478         alo=lastahi;
479         //xlo=current_control;
480         //alo=current_accel;
481       }     
482     }
483     lastxlo=xlo;lastxhi=xhi;
484     lastalo=alo;lastahi=ahi;
485     if( !found && xlo==xmin && xhi==xmax ) continue;
486     if(Debug > 1)
487       cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
488                            << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
489   } while(!found && (Nsub <= max_sub_iterations) );
490   return found;
491 }
492
493 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
494 //checks to see which side of the current control value the solution is on
495 //and sets solutionDomain accordingly:
496 //  1 if solution is between the current and max
497 // -1 if solution is between the min and current
498 //  0 if there is no solution
499 // 
500 //if changing the control produces no significant change in the accel then
501 //solutionDomain is set to zero and the function returns false
502 //if a solution is found, then xlo and xhi are set so that they bracket
503 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
504 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
505 //xhi=xmax and ahi=accel(xmax) 
506 //in all cases the sim is left such that the control=xmax and accel=ahi
507
508 bool FGTrim::checkLimits(void) {
509   bool solutionExists;
510   double current_control=TrimAxes[current_axis]->GetControl();
511   double current_accel=TrimAxes[current_axis]->GetState();
512   xlo=TrimAxes[current_axis]->GetControlMin();
513   xhi=TrimAxes[current_axis]->GetControlMax();
514
515   TrimAxes[current_axis]->SetControl(xlo);
516   TrimAxes[current_axis]->Run();
517   alo=TrimAxes[current_axis]->GetState();
518   TrimAxes[current_axis]->SetControl(xhi);
519   TrimAxes[current_axis]->Run();
520   ahi=TrimAxes[current_axis]->GetState();
521   if(Debug > 1)
522     cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
523                                               << alo << ", " << ahi << endl;
524   solutionDomain=0;
525   solutionExists=false;
526   if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
527     if(alo*current_accel <= 0) {
528       solutionExists=true;
529       solutionDomain=-1;
530       xhi=current_control;
531       ahi=current_accel;
532     } else if(current_accel*ahi < 0){
533       solutionExists=true;
534       solutionDomain=1;
535       xlo=current_control;
536       alo=current_accel;  
537     }
538   } 
539   TrimAxes[current_axis]->SetControl(current_control);
540   TrimAxes[current_axis]->Run();
541   return solutionExists;
542 }
543
544 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
545
546 void FGTrim::setupPullup() {
547   float g,q,cgamma;
548   FGColumnVector3 vPQR;
549   g=fdmex->GetInertial()->gravity();
550   cgamma=cos(fgic->GetFlightPathAngleRadIC());
551   cout << "setPitchRateInPullup():  " << g << ", " << cgamma << ", "
552        << fgic->GetVtrueFpsIC() << endl;
553   q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
554   cout << targetNlf << ", " << q << endl;
555   fdmex->GetRotation()->SetPQR(0,q,0);
556   cout << "setPitchRateInPullup() complete" << endl;
557   
558 }  
559
560 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
561
562 void FGTrim::setupTurn(void){
563   double g,phi;
564   phi = fgic->GetRollAngleRadIC();
565   if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
566     targetNlf = 1 / cos(phi);
567     g = fdmex->GetInertial()->gravity(); 
568     psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
569     cout << targetNlf << ", " << psidot << endl;
570   }
571    
572 }  
573
574 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
575
576 void FGTrim::updateRates(void){
577   if( mode == tTurn ) {
578     double phi = fgic->GetRollAngleRadIC();
579     double g = fdmex->GetInertial()->gravity(); 
580     double p,q,r,theta;
581     if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
582       theta=fgic->GetPitchAngleRadIC();
583       phi=fgic->GetRollAngleRadIC();
584       psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
585       p=-psidot*sin(theta);
586       q=psidot*cos(theta)*sin(phi);
587       r=psidot*cos(theta)*cos(phi);
588     } else {
589       p=q=r=0;
590     }      
591     fdmex->GetRotation()->SetPQR(p,q,r);
592   } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
593       float g,q,cgamma;
594       FGColumnVector3 vPQR;
595       g=fdmex->GetInertial()->gravity();
596       cgamma=cos(fgic->GetFlightPathAngleRadIC());
597       q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
598       fdmex->GetRotation()->SetPQR(0,q,0);
599   }  
600 }  
601
602 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
603
604 void FGTrim::setDebug(void) {
605   if(debug_axis == tAll ||
606       TrimAxes[current_axis]->GetStateType() == debug_axis ) {
607     Debug=DebugLevel; 
608     return;
609   } else {
610     Debug=0;
611     return;
612   }
613 }      
614
615 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
616
617 void FGTrim::SetMode(TrimMode tt) {
618     ClearStates();
619     switch(tt) {
620       case tFull:
621         cout << "  Full Trim" << endl;
622         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
623         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
624         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
625         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
626         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
627         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
628         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
629         break;
630       case tLongitudinal:
631         cout << "  Longitudinal Trim" << endl;
632         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
633         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
634         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
635         break;
636       case tGround:
637         cout << "  Ground Trim" << endl;
638         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
639         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
640         //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
641         break;
642       case tPullup:
643         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
644         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
645         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
646         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
647         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
648         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
649         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
650         break;
651       case tTurn:
652         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
653         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
654         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
655         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
656         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
657         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
658         break;
659       case tCustom:
660       case tNone:
661         break;
662     }
663     //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
664     sub_iterations=new double[TrimAxes.size()];
665     successful=new double[TrimAxes.size()];
666     solution=new bool[TrimAxes.size()];
667     current_axis=0;
668 }    
669 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
670 }