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