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