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