src/FDM/JSBSim/initialization/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 Lesser 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 Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26  HISTORY
  27 --------------------------------------------------------------------------------
  28 9/8/99   TP   Created
  29
  30 FUNCTIONAL DESCRIPTION
  31 --------------------------------------------------------------------------------
  32
  33 This class takes the given set of IC's and finds the angle of attack, elevator,
  34 and throttle setting required to fly steady level. This is currently for in-air
  35 conditions only.  It is implemented using an iterative, one-axis-at-a-time
  36 scheme. */
  37
  38 //  !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!
  39
  40 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  41 INCLUDES
  42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  43
  44 #include <cstdlib>
  45 #include <iomanip>
  46 #include "FGTrim.h"
  47 #include "models/FGAtmosphere.h"
  48 #include "FGInitialCondition.h"
  49 #include "models/FGAircraft.h"
  50 #include "models/FGMassBalance.h"
  51 #include "models/FGGroundReactions.h"
  52 #include "models/FGInertial.h"
  53 #include "models/FGAerodynamics.h"
  54 #include "math/FGColumnVector3.h"
  55
  56 #if _MSC_VER
  57 #pragma warning (disable : 4786 4788)
  58 #endif
  59
  60 using namespace std;
  61
  62 namespace JSBSim {
  63
  64 static const char *IdSrc = "$Id$";
  65 static const char *IdHdr = ID_TRIM;
  66
  67 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  68
  69 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
  70
  71   N=Nsub=0;
  72   max_iterations=60;
  73   max_sub_iterations=100;
  74   Tolerance=1E-3;
  75   A_Tolerance = Tolerance / 10;
  76
  77   Debug=0;DebugLevel=0;
  78   fdmex=FDMExec;
  79   fgic=fdmex->GetIC();
  80   total_its=0;
  81   trimudot=true;
  82   gamma_fallback=false;
  83   axis_count=0;
  84   mode=tt;
  85   xlo=xhi=alo=ahi=0.0;
  86   targetNlf=1.0;
  87   debug_axis=tAll;
  88   SetMode(tt);
  89   if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
  90 }
  91
  92 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  93
  94 FGTrim::~FGTrim(void) {
  95   for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
  96     delete TrimAxes[current_axis];
  97   }
  98   delete[] sub_iterations;
  99   delete[] successful;
 100   delete[] solution;
 101   if (debug_lvl & 2) cout << "Destroyed:    FGTrim" << endl;
 102 }
 103
 104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 105
 106 void FGTrim::TrimStats() {
 107   int run_sum=0;
 108   cout << endl << "  Trim Statistics: " << endl;
 109   cout << "    Total Iterations: " << total_its << endl;
 110   if( total_its > 0) {
 111     cout << "    Sub-iterations:" << endl;
 112     for (current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
 113       run_sum += TrimAxes[current_axis]->GetRunCount();
 114       cout << "   " << setw(5) << TrimAxes[current_axis]->GetStateName().c_str()
 115            << ": " << setprecision(3) << sub_iterations[current_axis]
 116            << " average: " << setprecision(5) << sub_iterations[current_axis]/double(total_its)
 117            << "  successful:  " << setprecision(3) << successful[current_axis]
 118            << "  stability: " << setprecision(5) << TrimAxes[current_axis]->GetAvgStability()
 119            << endl;
 120     }
 121     cout << "    Run Count: " << run_sum << endl;
 122   }
 123 }
 124
 125 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 126
 127 void FGTrim::Report(void) {
 128   cout << "  Trim Results: " << endl;
 129   for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
 130     TrimAxes[current_axis]->AxisReport();
 131
 132 }
 133
 134 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 135
 136 void FGTrim::ClearStates(void) {
 137     FGTrimAxis* ta;
 138
 139     mode=tCustom;
 140     vector<FGTrimAxis*>::iterator iAxes;
 141     iAxes = TrimAxes.begin();
 142     while (iAxes != TrimAxes.end()) {
 143       ta=*iAxes;
 144       delete ta;
 145       iAxes++;
 146     }
 147     TrimAxes.clear();
 148     //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
 149 }
 150
 151 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 152
 153 bool FGTrim::AddState( State state, Control control ) {
 154   FGTrimAxis* ta;
 155   bool result=true;
 156
 157   mode = tCustom;
 158   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
 159   while (iAxes != TrimAxes.end()) {
 160       ta=*iAxes;
 161       if( ta->GetStateType() == state )
 162         result=false;
 163       iAxes++;
 164   }
 165   if(result) {
 166     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
 167     delete[] sub_iterations;
 168     delete[] successful;
 169     delete[] solution;
 170     sub_iterations=new double[TrimAxes.size()];
 171     successful=new double[TrimAxes.size()];
 172     solution=new bool[TrimAxes.size()];
 173   }
 174   return result;
 175 }
 176
 177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 178
 179 bool FGTrim::RemoveState( State state ) {
 180   FGTrimAxis* ta;
 181   bool result=false;
 182
 183   mode = tCustom;
 184   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
 185   while (iAxes != TrimAxes.end()) {
 186       ta=*iAxes;
 187       if( ta->GetStateType() == state ) {
 188         delete ta;
 189         iAxes = TrimAxes.erase(iAxes);
 190         result=true;
 191         continue;
 192       }
 193       iAxes++;
 194   }
 195   if(result) {
 196     delete[] sub_iterations;
 197     delete[] successful;
 198     delete[] solution;
 199     sub_iterations=new double[TrimAxes.size()];
 200     successful=new double[TrimAxes.size()];
 201     solution=new bool[TrimAxes.size()];
 202   }
 203   return result;
 204 }
 205
 206 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 207
 208 bool FGTrim::EditState( State state, Control new_control ){
 209   FGTrimAxis* ta;
 210   bool result=false;
 211
 212   mode = tCustom;
 213   vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
 214   while (iAxes != TrimAxes.end()) {
 215       ta=*iAxes;
 216       if( ta->GetStateType() == state ) {
 217         TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
 218         delete ta;
 219         TrimAxes.erase(iAxes+1);
 220         result=true;
 221         break;
 222       }
 223       iAxes++;
 224   }
 225   return result;
 226 }
 227
 228
 229 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 230
 231 bool FGTrim::DoTrim(void) {
 232
 233   trim_failed=false;
 234   int i;
 235
 236   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
 237     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
 238   }
 239
 240   fdmex->DisableOutput();
 241
 242   fgic->SetPRadpsIC(0.0);
 243   fgic->SetQRadpsIC(0.0);
 244   fgic->SetRRadpsIC(0.0);
 245
 246   //clear the sub iterations counts & zero out the controls
 247   for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 248     //cout << current_axis << "  " << TrimAxes[current_axis]->GetStateName()
 249     //<< "  " << TrimAxes[current_axis]->GetControlName()<< endl;
 250     if(TrimAxes[current_axis]->GetStateType() == tQdot) {
 251       if(mode == tGround) {
 252           TrimAxes[current_axis]->initTheta();
 253       }
 254     }
 255     xlo=TrimAxes[current_axis]->GetControlMin();
 256     xhi=TrimAxes[current_axis]->GetControlMax();
 257     TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
 258     TrimAxes[current_axis]->Run();
 259     //TrimAxes[current_axis]->AxisReport();
 260     sub_iterations[current_axis]=0;
 261     successful[current_axis]=0;
 262     solution[current_axis]=false;
 263   }
 264
 265
 266   if(mode == tPullup ) {
 267     cout << "Setting pitch rate and nlf... " << endl;
 268     setupPullup();
 269     cout << "pitch rate done ... " << endl;
 270     TrimAxes[0]->SetStateTarget(targetNlf);
 271     cout << "nlf done" << endl;
 272   } else if (mode == tTurn) {
 273     setupTurn();
 274     //TrimAxes[0]->SetStateTarget(targetNlf);
 275   }
 276
 277   do {
 278     axis_count=0;
 279     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 280       setDebug();
 281       updateRates();
 282       Nsub=0;
 283       if(!solution[current_axis]) {
 284         if(checkLimits()) {
 285           solution[current_axis]=true;
 286           solve();
 287         }
 288       } else if(findInterval()) {
 289         solve();
 290       } else {
 291         solution[current_axis]=false;
 292       }
 293       sub_iterations[current_axis]+=Nsub;
 294     }
 295     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 296       //these checks need to be done after all the axes have run
 297       if(Debug > 0) TrimAxes[current_axis]->AxisReport();
 298       if(TrimAxes[current_axis]->InTolerance()) {
 299         axis_count++;
 300         successful[current_axis]++;
 301       }
 302     }
 303
 304
 305     if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
 306       //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
 307       //At this point we can check the input limits of the failed axis
 308       //and declare the trim failed if there is no sign change. If there
 309       //is, keep going until success or max iteration count
 310
 311       //Oh, well: two out of three ain't bad
 312       for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
 313         //these checks need to be done after all the axes have run
 314         if(!TrimAxes[current_axis]->InTolerance()) {
 315           if(!checkLimits()) {
 316             // special case this for now -- if other cases arise proper
 317             // support can be added to FGTrimAxis
 318             if( (gamma_fallback) &&
 319                 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
 320                 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
 321               cout << "  Can't trim udot with throttle, trying flight"
 322               << " path angle. (" << N << ")" << endl;
 323               if(TrimAxes[current_axis]->GetState() > 0)
 324                 TrimAxes[current_axis]->SetControlToMin();
 325               else
 326                 TrimAxes[current_axis]->SetControlToMax();
 327               TrimAxes[current_axis]->Run();
 328               delete TrimAxes[current_axis];
 329               TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
 330                                                     tGamma );
 331             } else {
 332               cout << "  Sorry, " << TrimAxes[current_axis]->GetStateName()
 333               << " doesn't appear to be trimmable" << endl;
 334               //total_its=k;
 335               trim_failed=true; //force the trim to fail
 336             } //gamma_fallback
 337           }
 338         } //solution check
 339       } //for loop
 340     } //all-but-one check
 341     N++;
 342     if(N > max_iterations)
 343       trim_failed=true;
 344   } while((axis_count < TrimAxes.size()) && (!trim_failed));
 345   if((!trim_failed) && (axis_count >= TrimAxes.size())) {
 346     total_its=N;
 347     if (debug_lvl > 0)
 348         cout << endl << "  Trim successful" << endl;
 349   } else {
 350     total_its=N;
 351     if (debug_lvl > 0)
 352         cout << endl << "  Trim failed" << endl;
 353   }
 354   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
 355     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
 356   }
 357   fdmex->EnableOutput();
 358   return !trim_failed;
 359 }
 360
 361 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 362
 363 bool FGTrim::solve(void) {
 364
 365   double x1,x2,x3,f1,f2,f3,d,d0;
 366   const double relax =0.9;
 367   double eps=TrimAxes[current_axis]->GetSolverEps();
 368
 369   x1=x2=x3=0;
 370   d=1;
 371   bool success=false;
 372   //initializations
 373   if( solutionDomain != 0) {
 374    /* if(ahi > alo) { */
 375       x1=xlo;f1=alo;
 376       x3=xhi;f3=ahi;
 377    /* } else {
 378       x1=xhi;f1=ahi;
 379       x3=xlo;f3=alo;
 380     }   */
 381     d0=fabs(x3-x1);
 382     //iterations
 383     //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
 384     while ( (TrimAxes[current_axis]->InTolerance() == false )
 385              && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
 386       Nsub++;
 387       d=(x3-x1)/d0;
 388       x2=x1-d*d0*f1/(f3-f1);
 389       TrimAxes[current_axis]->SetControl(x2);
 390       TrimAxes[current_axis]->Run();
 391       f2=TrimAxes[current_axis]->GetState();
 392       if(Debug > 1) {
 393         cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
 394         << ", " << x2 << ", " << x3 << endl;
 395         cout << "                             " << f1 << ", " << f2 << ", " << f3 << endl;
 396       }
 397       if(f1*f2 <= 0.0) {
 398         x3=x2;
 399         f3=f2;
 400         f1=relax*f1;
 401         //cout << "Solution is between x1 and x2" << endl;
 402       }
 403       else if(f2*f3 <= 0.0) {
 404         x1=x2;
 405         f1=f2;
 406         f3=relax*f3;
 407         //cout << "Solution is between x2 and x3" << endl;
 408
 409       }
 410       //cout << i << endl;
 411
 412
 413     }//end while
 414     if(Nsub < max_sub_iterations) success=true;
 415   }
 416   return success;
 417 }
 418
 419 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 420 /*
 421  produces an interval (xlo..xhi) on one side or the other of the current
 422  control value in which a solution exists.  This domain is, hopefully,
 423  smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
 424  to find the solution. This is, hopefully, more efficient than having the
 425  solver start from scratch every time. Maybe it isn't though...
 426  This tries to take advantage of the idea that the changes from iteration to
 427  iteration will be small after the first one or two top-level iterations.
 428
 429  assumes that changing the control will a produce significant change in the
 430  accel i.e. checkLimits() has already been called.
 431
 432  if a solution is found above the current control, the function returns true
 433  and xlo is set to the current control, xhi to the interval max it found, and
 434  solutionDomain is set to 1.
 435  if the solution lies below the current control, then the function returns
 436  true and xlo is set to the interval min it found and xmax to the current
 437  control. if no solution is found, then the function returns false.
 438
 439
 440  in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
 441  no assumptions about the state of the sim after this function has run
 442  can be made.
 443 */
 444 bool FGTrim::findInterval(void) {
 445   bool found=false;
 446   double step;
 447   double current_control=TrimAxes[current_axis]->GetControl();
 448   double current_accel=TrimAxes[current_axis]->GetState();;
 449   double xmin=TrimAxes[current_axis]->GetControlMin();
 450   double xmax=TrimAxes[current_axis]->GetControlMax();
 451   double lastxlo,lastxhi,lastalo,lastahi;
 452
 453   step=0.025*fabs(xmax);
 454   xlo=xhi=current_control;
 455   alo=ahi=current_accel;
 456   lastxlo=xlo;lastxhi=xhi;
 457   lastalo=alo;lastahi=ahi;
 458   do {
 459
 460     Nsub++;
 461     step*=2;
 462     xlo-=step;
 463     if(xlo < xmin) xlo=xmin;
 464     xhi+=step;
 465     if(xhi > xmax) xhi=xmax;
 466     TrimAxes[current_axis]->SetControl(xlo);
 467     TrimAxes[current_axis]->Run();
 468     alo=TrimAxes[current_axis]->GetState();
 469     TrimAxes[current_axis]->SetControl(xhi);
 470     TrimAxes[current_axis]->Run();
 471     ahi=TrimAxes[current_axis]->GetState();
 472     if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
 473     if(alo*ahi <=0) {  //found interval with root
 474       found=true;
 475       if(alo*current_accel <= 0) { //narrow interval down a bit
 476         solutionDomain=-1;
 477         xhi=lastxlo;
 478         ahi=lastalo;
 479         //xhi=current_control;
 480         //ahi=current_accel;
 481       } else {
 482         solutionDomain=1;
 483         xlo=lastxhi;
 484         alo=lastahi;
 485         //xlo=current_control;
 486         //alo=current_accel;
 487       }
 488     }
 489     lastxlo=xlo;lastxhi=xhi;
 490     lastalo=alo;lastahi=ahi;
 491     if( !found && xlo==xmin && xhi==xmax ) continue;
 492     if(Debug > 1)
 493       cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
 494                            << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
 495   } while(!found && (Nsub <= max_sub_iterations) );
 496   return found;
 497 }
 498
 499 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 500 //checks to see which side of the current control value the solution is on
 501 //and sets solutionDomain accordingly:
 502 //  1 if solution is between the current and max
 503 // -1 if solution is between the min and current
 504 //  0 if there is no solution
 505 //
 506 //if changing the control produces no significant change in the accel then
 507 //solutionDomain is set to zero and the function returns false
 508 //if a solution is found, then xlo and xhi are set so that they bracket
 509 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
 510 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
 511 //xhi=xmax and ahi=accel(xmax)
 512 //in all cases the sim is left such that the control=xmax and accel=ahi
 513
 514 bool FGTrim::checkLimits(void) {
 515   bool solutionExists;
 516   double current_control=TrimAxes[current_axis]->GetControl();
 517   double current_accel=TrimAxes[current_axis]->GetState();
 518   xlo=TrimAxes[current_axis]->GetControlMin();
 519   xhi=TrimAxes[current_axis]->GetControlMax();
 520
 521   TrimAxes[current_axis]->SetControl(xlo);
 522   TrimAxes[current_axis]->Run();
 523   alo=TrimAxes[current_axis]->GetState();
 524   TrimAxes[current_axis]->SetControl(xhi);
 525   TrimAxes[current_axis]->Run();
 526   ahi=TrimAxes[current_axis]->GetState();
 527   if(Debug > 1)
 528     cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
 529                                               << alo << ", " << ahi << endl;
 530   solutionDomain=0;
 531   solutionExists=false;
 532   if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
 533     if(alo*current_accel <= 0) {
 534       solutionExists=true;
 535       solutionDomain=-1;
 536       xhi=current_control;
 537       ahi=current_accel;
 538     } else if(current_accel*ahi < 0){
 539       solutionExists=true;
 540       solutionDomain=1;
 541       xlo=current_control;
 542       alo=current_accel;
 543     }
 544   }
 545   TrimAxes[current_axis]->SetControl(current_control);
 546   TrimAxes[current_axis]->Run();
 547   return solutionExists;
 548 }
 549
 550 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 551
 552 void FGTrim::setupPullup() {
 553   double g,q,cgamma;
 554   g=fdmex->GetInertial()->gravity();
 555   cgamma=cos(fgic->GetFlightPathAngleRadIC());
 556   cout << "setPitchRateInPullup():  " << g << ", " << cgamma << ", "
 557        << fgic->GetVtrueFpsIC() << endl;
 558   q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
 559   cout << targetNlf << ", " << q << endl;
 560   fgic->SetQRadpsIC(q);
 561   cout << "setPitchRateInPullup() complete" << endl;
 562
 563 }
 564
 565 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 566
 567 void FGTrim::setupTurn(void){
 568   double g,phi;
 569   phi = fgic->GetPhiRadIC();
 570   if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
 571     targetNlf = 1 / cos(phi);
 572     g = fdmex->GetInertial()->gravity();
 573     psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
 574     cout << targetNlf << ", " << psidot << endl;
 575   }
 576
 577 }
 578
 579 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 580
 581 void FGTrim::updateRates(void){
 582   if( mode == tTurn ) {
 583     double phi = fgic->GetPhiRadIC();
 584     double g = fdmex->GetInertial()->gravity();
 585     double p,q,r,theta;
 586     if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
 587       theta=fgic->GetThetaRadIC();
 588       phi=fgic->GetPhiRadIC();
 589       psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
 590       p=-psidot*sin(theta);
 591       q=psidot*cos(theta)*sin(phi);
 592       r=psidot*cos(theta)*cos(phi);
 593     } else {
 594       p=q=r=0;
 595     }
 596     fgic->SetPRadpsIC(p);
 597     fgic->SetQRadpsIC(q);
 598     fgic->SetRRadpsIC(r);
 599   } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
 600       double g,q,cgamma;
 601       g=fdmex->GetInertial()->gravity();
 602       cgamma=cos(fgic->GetFlightPathAngleRadIC());
 603       q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
 604       fgic->SetQRadpsIC(q);
 605   }
 606 }
 607
 608 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 609
 610 void FGTrim::setDebug(void) {
 611   if(debug_axis == tAll ||
 612       TrimAxes[current_axis]->GetStateType() == debug_axis ) {
 613     Debug=DebugLevel;
 614     return;
 615   } else {
 616     Debug=0;
 617     return;
 618   }
 619 }
 620
 621 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 622
 623 void FGTrim::SetMode(TrimMode tt) {
 624     ClearStates();
 625     mode=tt;
 626     switch(tt) {
 627       case tFull:
 628         if (debug_lvl > 0)
 629           cout << "  Full Trim" << endl;
 630         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
 631         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 632         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 633         //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
 634         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
 635         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
 636         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
 637         break;
 638       case tLongitudinal:
 639         if (debug_lvl > 0)
 640           cout << "  Longitudinal Trim" << endl;
 641         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
 642         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 643         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 644         break;
 645       case tGround:
 646         if (debug_lvl > 0)
 647           cout << "  Ground Trim" << endl;
 648         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
 649         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
 650         //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
 651         break;
 652       case tPullup:
 653         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
 654         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 655         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 656         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
 657         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
 658         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
 659         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
 660         break;
 661       case tTurn:
 662         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
 663         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
 664         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
 665         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
 666         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
 667         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
 668         break;
 669       case tCustom:
 670       case tNone:
 671         break;
 672     }
 673     //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
 674     sub_iterations=new double[TrimAxes.size()];
 675     successful=new double[TrimAxes.size()];
 676     solution=new bool[TrimAxes.size()];
 677     current_axis=0;
 678 }
 679 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
 680 }