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