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