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