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